GCS_MAVLink: report sequence based rx loss with SYS_STATUS

bugobliterator · Aug 30, 2024 · 1aa14a0 · 1aa14a0
1 parent f7bfa5d
commit 1aa14a0
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 7 deletions.
diff --git 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
@@ -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