Re-architect mavlink UDP forwarder (#154)

* Re-architect mavlink-UDP code to support future telem forwarding options

Author: wvarty

html/log.html

@@ -3,49 +3,62 @@
 #include "MAVLink.h"
 #include <config.h>
 
-// Returns whether the message is a control message, i.e. a message where we don't want to
-// pass the message to the flight controller, but rather handle it ourselves. If the message
-// is a control message, the function handles it internally.
-bool MAVLink::handleControlMessage(mavlink_message_t *msg)
+void
+MAVLink::ProcessMAVLinkFromTX(uint8_t c)
 {
-    bool shouldForward = true;
-    // Check for messages addressed to the Backpack
-    switch (msg->msgid)
+    mavlink_status_t status;
+    mavlink_message_t msg;
+
+    if (mavlink_frame_char(MAVLINK_COMM_0, c, &msg, &status) != MAVLINK_FRAMING_INCOMPLETE)
     {
-    case MAVLINK_MSG_ID_COMMAND_INT:
-        mavlink_command_int_t commandMsg;
-        mavlink_msg_command_int_decode(msg, &commandMsg);
-        if (commandMsg.target_component == MAV_COMP_ID_UDP_BRIDGE)
+        if (mavlink_to_gcs_buf_count >= MAVLINK_BUF_SIZE)
+        {
+            // Cant fit any more msgs in the queue,
+            // drop the oldest msg and start overwriting
+            mavlink_stats.overflows_downlink++;
+            mavlink_to_gcs_buf_count = 0;
+        }
+        
+        // Track gaps in the sequence number, add to a dropped counter
+        uint8_t seq = msg.seq;
+        if (expectedSeqSet && seq != expectedSeq)
         {
-            shouldForward = false;
-            constexpr uint8_t ELRS_MODE_CHANGE = 0x8;
-            switch (commandMsg.command)
+            // account for rollovers
+            if (seq < expectedSeq)
             {
-            case MAV_CMD_USER_1:
-                switch ((int)commandMsg.param1)
-                {
-                case ELRS_MODE_CHANGE:
-                    switch ((int)commandMsg.param2)
-                    {
-                    case 0: // TX_NORMAL_MODE
-                        config.SetStartWiFiOnBoot(false);
-                        ESP.restart();
-                        break;
-                    case 1: // TX_MAVLINK_MODE
-                        if (config.GetWiFiService() != WIFI_SERVICE_MAVLINK_TX)
-                        {
-                            config.SetWiFiService(WIFI_SERVICE_MAVLINK_TX);
-                            config.SetStartWiFiOnBoot(true);
-                            config.Commit();
-                            ESP.restart();
-                        }
-                        break;
-                    }
-                }
+                mavlink_stats.drops_downlink += (UINT8_MAX - expectedSeq) + seq;
             }
-            break;
+            else
+            {
+                mavlink_stats.drops_downlink += seq - expectedSeq;
+            }
+        }
+        expectedSeq = seq + 1;
+        expectedSeqSet = true;
+
+        // Queue the msgs, to forward to peers
+        mavlink_to_gcs_buf[mavlink_to_gcs_buf_count] = msg;
+        mavlink_to_gcs_buf_count++;
+        mavlink_stats.packets_downlink++;
+    }
+}
+
+void
+MAVLink::ProcessMAVLinkFromGCS(uint8_t *data, uint16_t len)
+{
+    mavlink_status_t status;
+    mavlink_message_t msg;
+
+    for (uint16_t i = 0; i < len; i++)
+    {
+        if (mavlink_frame_char(MAVLINK_COMM_1, data[i], &msg, &status) != MAVLINK_FRAMING_INCOMPLETE)
+        {
+            // Send the message to the tx uart
+            uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+            uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+            Serial.write(buf, len);
+            mavlink_stats.packets_uplink++;
         }
     }
-    return shouldForward;
 }
-#endif
+#endif

html/log.js

@@ -1,9 +1,45 @@
 #if defined(MAVLINK_ENABLED)
 #include "common/mavlink.h"
 
+// This is the port that we will listen for mavlink packets on
+constexpr uint16_t MAVLINK_PORT_LISTEN = 14555;
+// This is the port that we will send mavlink packets to
+constexpr uint16_t MAVLINK_PORT_SEND = 14550;
+// Size of the buffer that we will use to store mavlink packets in units of mavlink_message_t
+constexpr size_t MAVLINK_BUF_SIZE = 16;
+// Threshold at which we will flush the buffer
+constexpr size_t MAVLINK_BUF_THRESHOLD = MAVLINK_BUF_SIZE / 2;
+// Timeout for flushing the buffer in ms
+constexpr size_t MAVLINK_BUF_TIMEOUT = 500;
+
+typedef struct {
+  uint32_t packets_downlink; // packets from the aircraft
+  uint32_t packets_uplink; // packets to the aircraft
+  uint32_t drops_downlink; // dropped packets from the aircraft
+  //uint32_t drops_uplink; // framing in the uplink direction cost too much time
+  uint32_t overflows_downlink; // buffer overflows from the aircraft
+} mavlink_stats_t;
+
 class MAVLink
 {
 public:
-    static bool handleControlMessage(mavlink_message_t* msg);
+    MAVLink() : mavlink_to_gcs_buf_count(0), expectedSeq(0), expectedSeqSet(false) {}
+    void ProcessMAVLinkFromTX(uint8_t c);
+    void ProcessMAVLinkFromGCS(uint8_t *data, uint16_t len);
+
+    // Getters
+    mavlink_stats_t* GetMavlinkStats() { return &mavlink_stats; }
+    mavlink_message_t* GetQueuedMsgs() { return mavlink_to_gcs_buf; }
+    uint8_t GetQueuedMsgCount() { return mavlink_to_gcs_buf_count; }
+
+    // Setters
+    void ResetQueuedMsgCount() { mavlink_to_gcs_buf_count = 0; }
+
+private:
+    mavlink_stats_t     mavlink_stats;
+    mavlink_message_t   mavlink_to_gcs_buf[MAVLINK_BUF_SIZE]; // Buffer for storing mavlink packets from the aircraft so that we can send them to the GCS efficiently
+    uint8_t             mavlink_to_gcs_buf_count; // Count of the number of messages in the buffer
+    uint8_t             expectedSeq;
+    bool                expectedSeqSet;
 };
-#endif
+#endif

lib/MAVLink/MAVLink.cpp

@@ -23,6 +23,10 @@
 #define MSP_ELRS_SET_SEND_UID                   0x00B5
 #define MSP_ELRS_SET_OSD                        0x00B6
 
+// Config opcodes
+#define MSP_ELRS_BACKPACK_CONFIG                0x30
+#define MSP_ELRS_BACKPACK_CONFIG_TLM_MODE       0x31
+
 // CRSF encapsulated msp defines
 #define ENCAPSULATED_MSP_PAYLOAD_SIZE           4
 #define ENCAPSULATED_MSP_FRAME_LEN              8