diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index a720766a4b9f7c..4c5fc5e9716c8d 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -510,6 +510,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 72b238d1ff03ea..7c2d9670fb521e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1831,6 +1831,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; @@ -5504,13 +5517,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