ardupilot/Tools/ArduTracker/HIL_Mavlink.pde

155 lines
3.7 KiB
Plaintext
Raw Normal View History

2011-09-08 22:31:32 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
#include "Mavlink_Common.h"
HIL_MAVLINK::HIL_MAVLINK() :
packet_drops(0)
{
}
void
HIL_MAVLINK::init(BetterStream * port)
{
HIL_Class::init(port);
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0;
}
void
HIL_MAVLINK::update(void)
{
mavlink_message_t msg;
mavlink_status_t status;
// process received bytes
while(comm_get_available(chan))
{
uint8_t c = comm_receive_ch(chan);
// Try to get a new message
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
}
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
}
void
HIL_MAVLINK::send_message(uint8_t id, uint32_t param)
{
mavlink_send_message(chan,id,param,packet_drops);
}
void
HIL_MAVLINK::send_text(uint8_t severity, const char *str)
{
mavlink_send_text(chan,severity,str);
}
void
HIL_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
mavlink_acknowledge(chan,id,sum1,sum2);
}
void
HIL_MAVLINK::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
// handle incoming vehicle position
case MAVLINK_MSG_ID_GLOBAL_POSITION:
{
// decode
mavlink_global_position_t packet;
mavlink_msg_global_position_decode(msg, &packet);
//if (mavlink_check_target(packet.target_system,packet.target_component)) break;
trackVehicle_loc.id = 0;
trackVehicle_loc.p1 = 0;
trackVehicle_loc.alt = packet.alt;
trackVehicle_loc.lat = packet.lat;
trackVehicle_loc.lng = packet.lon;
Serial.println("received global position packet");
}
// This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode.
case MAVLINK_MSG_ID_GPS_RAW:
{
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
// set gps hil sensor
gps.setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
packet.v,packet.hdg,0,0);
break;
}
case MAVLINK_MSG_ID_AIRSPEED:
{
// decode
mavlink_airspeed_t packet;
mavlink_msg_airspeed_decode(msg, &packet);
// set airspeed
airspeed = 100*packet.airspeed;
break;
}
#if HIL_MODE == HIL_MODE_ATTITUDE
case MAVLINK_MSG_ID_ATTITUDE:
{
// decode
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
// set gps hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
break;
}
#elif HIL_MODE == HIL_MODE_SENSORS
case MAVLINK_MSG_ID_RAW_IMU:
{
// decode
mavlink_raw_imu_t packet;
mavlink_msg_raw_imu_decode(msg, &packet);
// set imu hil sensors
// TODO: check scaling for temp/absPress
float temp = 70;
float absPress = 1;
//Serial.printf_P(PSTR("\nreceived accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
//Serial.printf_P(PSTR("\nreceived gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
adc.setHIL(packet.xgyro,packet.ygyro,packet.zgyro,temp,
packet.xacc,packet.yacc,packet.zacc,absPress);
compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
break;
}
case MAVLINK_MSG_ID_RAW_PRESSURE:
{
// decode
mavlink_raw_pressure_t packet;
mavlink_msg_raw_pressure_decode(msg, &packet);
// set pressure hil sensor
// TODO: check scaling
float temp = 70;
pitot.setHIL(temp,packet.press_diff1);
break;
}
#endif // HIL_MODE
} // end switch
}
#endif