diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b1e2a60fe6..c0cb7029c7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -128,6 +128,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _land_detector_pub(nullptr), _time_offset_pub(nullptr), _follow_target_pub(nullptr), + _transponder_report_pub(nullptr), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -228,9 +229,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_DISTANCE_SENSOR: handle_message_distance_sensor(msg); break; + case MAVLINK_MSG_ID_FOLLOW_TARGET: handle_message_follow_target(msg); break; + + case MAVLINK_MSG_ID_ADSB_VEHICLE: + handle_message_adsb_vehicle(msg); + break; default: break; } @@ -1646,6 +1652,38 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) } } +void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) +{ + mavlink_adsb_vehicle_t adsb; + transponder_report_s t = { }; + + mavlink_msg_adsb_vehicle_decode(msg, &adsb); + + t.timestamp = hrt_absolute_time(); + + t.ICAO_address = adsb.ICAO_address; + t.lat = adsb.lat*1e-7; + t.lon = adsb.lon*1e-7; + t.altitude_type = adsb.altitude_type; + t.altitude = adsb.altitude / 1000.0f; + t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F - M_PI_F; + t.hor_velocity = adsb.hor_velocity / 100.0f; + t.ver_velocity = adsb.ver_velocity / 100.0f; + memcpy(&t.callsign[0], &adsb.callsign[0], sizeof(t.callsign)); + t.emitter_type = adsb.emitter_type; + t.tslc = adsb.tslc; + t.flags = adsb.flags; + t.squawk = adsb.squawk; + + //warnx("code: %d callsign: %s, vel: %8.4f", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity); + + if (_transponder_report_pub == nullptr) { + _transponder_report_pub = orb_advertise(ORB_ID(transponder_report), &t); + } else { + orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t); + } +} + void MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9a4ddc7257..0e99568176 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -75,6 +75,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -138,6 +139,7 @@ private: void handle_message_hil_state_quaternion(mavlink_message_t *msg); void handle_message_distance_sensor(mavlink_message_t *msg); void handle_message_follow_target(mavlink_message_t *msg); + void handle_message_adsb_vehicle(mavlink_message_t *msg); void *receive_thread(void *arg); @@ -200,6 +202,7 @@ private: orb_advert_t _land_detector_pub; orb_advert_t _time_offset_pub; orb_advert_t _follow_target_pub; + orb_advert_t _transponder_report_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp;