MAVLink app: Read MAVLink transponder report

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)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -75,6 +75,7 @@
 #include <uORB/topics/time_offset.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/follow_target.h>
+#include <uORB/topics/transponder_report.h>
 
 #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;