From 1aa14a091b185b9cdb0948102c91925e3c28746d Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 3 Feb 2020 13:34:14 +0530 Subject: [PATCH] GCS_MAVLink: report sequence based rx loss with SYS_STATUS --- libraries/GCS_MAVLink/GCS.h | 3 +++ libraries/GCS_MAVLink/GCS_Common.cpp | 27 ++++++++++++++++++++------- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 95cc4237fe2eed..b9da2ddbc22a86 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -507,6 +507,9 @@ class GCS_MAVLINK AP_Param * _queued_parameter; ///< next parameter to // be sent in queue mavlink_channel_t chan; + uint32_t lost_msg_cnt; + uint32_t total_msg_cnt; + uint8_t last_gcs_msg_seq; uint8_t packet_overhead(void) const { return packet_overhead_chan(chan); } // saveable rate of each stream diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ac7dd381335b4b..00fa51ab50b692 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1757,7 +1757,12 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, // MAVLink2 _channel_status.flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; } +<<<<<<< HEAD if (!routing.check_and_forward(*this, msg)) { +======= + + if (!routing.check_and_forward(chan, msg)) { +>>>>>>> f766a37e07 (GCS_MAVLink: report sequence based rx loss with SYS_STATUS) // the routing code has indicated we should not handle this packet locally return; } @@ -1831,6 +1836,19 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us) if (mavlink_frame_char_buffer(channel_buffer(), channel_status(), c, &msg, &status) == MAVLINK_FRAMING_OK) { hal.util->persistent_data.last_mavlink_msgid = msg.msgid; packetReceived(status, msg); +<<<<<<< HEAD +======= + if (msg.sysid == sysid_my_gcs() && total_msg_cnt != 0) { + lost_msg_cnt += uint8_t(msg.seq - last_gcs_msg_seq - 1); + total_msg_cnt += uint8_t(msg.seq - last_gcs_msg_seq); + last_gcs_msg_seq = msg.seq; + } else if (total_msg_cnt == 0) { + lost_msg_cnt = 0; + total_msg_cnt = 1; + last_gcs_msg_seq = msg.seq; + } + hal.util->perf_end(_perf_packet); +>>>>>>> f766a37e07 (GCS_MAVLink: report sequence based rx loss with SYS_STATUS) parsed_packet = true; gcs_alternative_active[chan] = false; alternative.last_mavlink_ms = now_ms; @@ -5503,13 +5521,8 @@ void GCS_MAVLINK::send_sys_status() battery.gcs_voltage() * 1000, // mV battery_current, // in 10mA units battery_remaining, // in % -#else - 0, - -1, - -1, -#endif - 0, // comm drops %, - 0, // comm drops in pkts, + (lost_msg_cnt*100)/((total_msg_cnt==0)?1:total_msg_cnt), // comm drops %, + lost_msg_cnt, // comm drops in pkts, errors1, errors2, 0, // errors3