Fixes packet drops & adds some counters - still a few changes to make

Author: MUSTARDTIGERFPV

lib/WIFI/devWIFI.cpp

@@ -109,9 +109,10 @@ uint32_t mavlink_from_uart_packets = 0;
 uint32_t mavlink_to_uart_packets = 0;
 uint32_t mavlink_from_uart_drops = 0;
 uint32_t mavlink_to_uart_drops = 0;
+uint32_t mavlink_from_uart_overflows = 0;
 constexpr size_t mavlink_to_uart_buf_size = 16;
-constexpr size_t mavlink_to_uart_buf_threshold = mavlink_to_uart_buf_size / 4;
-constexpr size_t mavlink_to_uart_buf_timeout = 5; // 5ms
+constexpr size_t mavlink_to_uart_buf_threshold = mavlink_to_uart_buf_size / 2;
+constexpr size_t mavlink_to_uart_buf_timeout = 500; // ms
 mavlink_message_t mavlink_to_gcs_buf[mavlink_to_uart_buf_size];
 uint8_t mavlink_to_gcs_buf_count = 0;
 #endif
@@ -476,10 +477,11 @@ static void WebMAVLinkHandler(AsyncWebServerRequest *request)
 {
   DynamicJsonDocument json(1024);
   json["enabled"] = wifiService == WIFI_SERVICE_MAVLINK_TX;
-  json["counters"]["packets_to_gcs"] = mavlink_from_uart_packets;
-  json["counters"]["packets_to_aircraft"] = mavlink_to_uart_packets;
-  json["counters"]["drops_to_gcs"] = mavlink_from_uart_drops;
-  json["counters"]["drops_to_aircraft"] = mavlink_to_uart_drops;
+  json["counters"]["packets_from_aircraft"] = mavlink_from_uart_packets;
+  json["counters"]["packets_from_gcs"] = mavlink_to_uart_packets;
+  json["counters"]["drops_from_aircraft"] = mavlink_from_uart_drops;
+  json["counters"]["drops_from_gcs"] = mavlink_to_uart_drops;
+  json["counters"]["overflows_from_aircraft"] = mavlink_from_uart_overflows;
   json["ports"]["listen"] = MAVLINK_PORT_LISTEN;
   json["ports"]["send"] = MAVLINK_PORT_SEND;
 
@@ -726,39 +728,58 @@ static void HandleWebUpdate()
 
   if (servicesStarted)
   {
-    while (Serial.available()) {
+    while (Serial.available())
+    {
 #if defined(MAVLINK_ENABLED)
-      if (wifiService == WIFI_SERVICE_UPDATE) {
+      if (wifiService == WIFI_SERVICE_UPDATE)
+      {
         int val = Serial.read();
         logBuffer[logPos++] = val;
         logBuffer[logPos] = 0;
-        if (val == '\n' || logPos == sizeof(logBuffer)-1) {
+        if (val == '\n' || logPos == sizeof(logBuffer) - 1)
+        {
           logging.send(logBuffer);
           logPos = 0;
         }
-      } else if (wifiService == WIFI_SERVICE_MAVLINK_TX) {
-        mavlink_message_t msg;
+      }
+      else if (wifiService == WIFI_SERVICE_MAVLINK_TX)
+      {
         mavlink_status_t status;
         char val = Serial.read();
-        static uint8_t seq = 0;
-        if (mavlink_parse_char(MAVLINK_COMM_0, val, &msg, &status)) {
+        static uint8_t expectedSeq = 0;
+        static bool expectedSeqSet = false;
+        if (mavlink_to_gcs_buf_count >= mavlink_to_uart_buf_size)
+        {
+          mavlink_from_uart_overflows++;
+          return;
+        }
+        if (mavlink_frame_char(MAVLINK_COMM_0, val, &mavlink_to_gcs_buf[mavlink_to_gcs_buf_count], &status) != MAVLINK_FRAMING_INCOMPLETE)
+        {
           // Track gaps in the sequence number, add to a dropped counter
-          if (seq != 0 && msg.seq != seq + 1) {
-            mavlink_from_uart_drops += msg.seq - seq - 1;
-          }
-          seq = msg.seq;
-          if (mavlink_to_gcs_buf_count < mavlink_to_uart_buf_size) {
-            mavlink_to_gcs_buf[mavlink_to_gcs_buf_count++] = msg;
-          } else {
-            // TODO: log buffer overflow
+          uint8_t seq = mavlink_to_gcs_buf[mavlink_to_gcs_buf_count].seq;
+          if (expectedSeqSet && seq != expectedSeq)
+          {
+            // account for rollovers
+            if (seq < expectedSeq)
+            {
+              mavlink_from_uart_drops += (UINT8_MAX - expectedSeq) + seq;
+            }
+            else
+            {
+              mavlink_from_uart_drops += seq - expectedSeq;
+            }
           }
+          expectedSeq = seq + 1;
+          expectedSeqSet = true;
+          mavlink_to_gcs_buf_count++;
         }
       }
 #else
       int val = Serial.read();
       logBuffer[logPos++] = val;
       logBuffer[logPos] = 0;
-      if (val == '\n' || logPos == sizeof(logBuffer)-1) {
+      if (val == '\n' || logPos == sizeof(logBuffer) - 1)
+      {
         logging.send(logBuffer);
         logPos = 0;
       }
@@ -785,7 +806,6 @@ static void HandleWebUpdate()
         {
           break;
           mavlinkUDP.endPacket();
-          delay(2);
           mavlinkUDP.beginPacket(broadcast, MAVLINK_PORT_SEND);
           sent = mavlinkUDP.write(buf + sent, len - sent);
           mavlinkUDP.endPacket();
@@ -815,7 +835,7 @@ static void HandleWebUpdate()
     #endif
     // When in STA mode, a small delay reduces power use from 90mA to 30mA when idle
     // In AP mode, it doesn't seem to make a measurable difference, but does not hurt
-    if (!updater.isRunning())
+    if (!updater.isRunning() && wifiService != WIFI_SERVICE_MAVLINK_TX)
       delay(1);
     if (do_flash) {
       do_flash = false;

src/Tx_main.cpp

@@ -342,7 +342,7 @@ void setup()
     Serial1.setDebugOutput(true);
   #endif
   Serial.begin(460800);
-  Serial.setRxBufferSize(1024);
+  Serial.setRxBufferSize(4096);
 
   options_init();