From c1f5feac831776b5721c0f400782faf7b784c848 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Jul 2017 08:31:08 +0200 Subject: [PATCH] MAVLink: Queue transponder reports and send them at full data rate This will ensure that no transponder reports are dropped and that all received reports are passed on to the GCS and other devices. --- msg/transponder_report.msg | 2 ++ src/modules/mavlink/mavlink_messages.cpp | 7 ++++++- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg index c02fb8cc15..3d13a42d4a 100644 --- a/msg/transponder_report.msg +++ b/msg/transponder_report.msg @@ -11,3 +11,5 @@ uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum uint8 tslc # Time since last communication in seconds uint16 flags # Flags to indicate various statuses including valid data fields uint16 squawk # Squawk code + +uint32 ORB_QUEUE_LENGTH = 3 diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 542a444e4b..31d9c0bf7c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1178,6 +1178,11 @@ public: return new MavlinkStreamADSBVehicle(mavlink); } + virtual bool const_rate() + { + return true; + } + unsigned get_size() { return (_pos_time > 0) ? MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; @@ -1201,7 +1206,7 @@ protected: { struct transponder_report_s pos; - if (_pos_sub->update(&_pos_time, &pos)) { + while (_pos_sub->update(&_pos_time, &pos)) { mavlink_adsb_vehicle_t msg = {}; msg.ICAO_address = pos.ICAO_address; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 154b23ba13..ef958194b4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2087,7 +2087,7 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) //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); + _transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH); } else { orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);