Add MAVLink support to Backpack

lib/BUTTON/devButton.cpp

@@ -1,6 +1,7 @@
 #include <Arduino.h>
 #include "common.h"
 #include "device.h"
+#include "config.h"
 
 #if defined(PIN_BUTTON)
 #include "logging.h"
@@ -9,7 +10,7 @@
 static Button<PIN_BUTTON, false> button;
 
 extern unsigned long rebootTime;
-void RebootIntoWifi();
+void RebootIntoWifi(wifi_service_t service);
 
 static void shortPress()
 {
@@ -19,7 +20,7 @@ static void shortPress()
     }
     else
     {
-        RebootIntoWifi();
+        RebootIntoWifi(WIFI_SERVICE_UPDATE);
     }
 }
 

lib/WIFI/devWIFI.cpp

@@ -33,11 +33,16 @@
 #include "WebContent.h"
 
 #include "config.h"
+
+#if defined(MAVLINK_ENABLED)
+#include "common/mavlink.h"
+#endif
 #if defined(TARGET_VRX_BACKPACK)
 extern VrxBackpackConfig config;
 extern bool sendRTCChangesToVrx;
 #elif defined(TARGET_TX_BACKPACK)
 extern TxBackpackConfig config;
+extern wifi_service_t wifiService;
 #elif defined(TARGET_TIMER_BACKPACK)
 extern TimerBackpackConfig config;
 #else
@@ -50,7 +55,8 @@ static const char *myHostname = "elrs_vrx";
 static const char *wifi_ap_ssid = "ExpressLRS VRx Backpack";
 #elif defined(TARGET_TX_BACKPACK)
 static const char *myHostname = "elrs_txbp";
-static const char *wifi_ap_ssid = "ExpressLRS TX Backpack";
+static char wifi_ap_ssid[33]; // will be set depending on wifiService
+
 #elif defined(TARGET_TIMER_BACKPACK)
 static const char *myHostname = "elrs_timer";
 static const char *wifi_ap_ssid = "ExpressLRS Timer Backpack";
@@ -74,6 +80,7 @@ static volatile unsigned long changeTime = 0;
 static const byte DNS_PORT = 53;
 static IPAddress apIP(10, 0, 0, 1);
 static IPAddress netMsk(255, 255, 255, 0);
+static IPAddress apBroadcast(10, 0, 0, 255);
 static DNSServer dnsServer;
 
 static AsyncWebServer server(80);
@@ -92,6 +99,16 @@ static AsyncEventSource logging("/logging");
 static char logBuffer[256];
 static int logPos = 0;
 
+#if defined(MAVLINK_ENABLED)
+// This is the port that we will listen for mavlink packets on
+static constexpr uint16_t MAVLINK_PORT_LISTEN = 14555;
+// This is the port that we will send mavlink packets to
+static constexpr uint16_t MAVLINK_PORT_SEND = 14550;
+WiFiUDP mavlinkUDP;
+uint32_t mavlink_from_uart_counter = 0;
+uint32_t mavlink_to_uart_counter = 0;
+#endif
+
 /** Is this an IP? */
 static boolean isIp(String str)
 {
@@ -447,6 +464,29 @@ static void WebUploadRTCUpdateHandler(AsyncWebServerRequest *request) {
   #endif
 }
 
+static void WebMAVLinkHandler(AsyncWebServerRequest *request)
+{
+#if defined(PLATFORM_ESP32)
+  DynamicJsonDocument json(32768);
+#else
+  DynamicJsonDocument json(2048);
+#endif
+
+  json["counters"]["from_uart"] = mavlink_from_uart_counter;
+  json["counters"]["to_uart"] = mavlink_to_uart_counter;
+
+#if defined(STM32_TX_BACKPACK)
+  json["stm32"] = "yes";
+#endif
+#if defined(AAT_BACKPACK)
+  WebAatAppendConfig(json);
+#endif
+
+  AsyncResponseStream *response = request->beginResponseStream("application/json");
+  serializeJson(json, *response);
+  request->send(response);
+}
+
 static void wifiOff()
 {
   wifiStarted = false;
@@ -586,6 +626,7 @@ static void startServices()
 #if defined(AAT_BACKPACK)
   WebAatInit(server);
 #endif
+  server.on("/mavlink", HTTP_GET, WebMAVLinkHandler);
 
   server.on("/log.js", WebUpdateSendContent);
   server.on("/log.html", WebUpdateSendContent);
@@ -600,6 +641,8 @@ static void startServices()
 
   startMDNS();
 
+  mavlinkUDP.begin(MAVLINK_PORT_LISTEN);
+
   servicesStarted = true;
   DBGLN("HTTPUpdateServer ready! Open http://%s.local in your browser", myHostname);
 }
@@ -643,6 +686,18 @@ static void HandleWebUpdate()
         #endif
         changeTime = now;
         WiFi.softAPConfig(apIP, apIP, netMsk);
+#if defined(TARGET_TX_BACKPACK)
+        if (wifiService == WIFI_SERVICE_UPDATE) {
+          strcpy(wifi_ap_ssid, "ExpressLRS TX Backpack");
+        } else if (wifiService == WIFI_SERVICE_MAVLINK_TX) {
+          // Generate a unique SSID using config.address as hex
+          sprintf(wifi_ap_ssid, "ExpressLRS TX %02X%02X%02X",
+            firmwareOptions.uid[3],
+            firmwareOptions.uid[4],
+            firmwareOptions.uid[5]
+          );
+        }
+#endif
         WiFi.softAP(wifi_ap_ssid, wifi_ap_password);
         WiFi.scanNetworks(true);
         startServices();
@@ -667,12 +722,40 @@ static void HandleWebUpdate()
   if (servicesStarted)
   {
     while (Serial.available()) {
-      int val = Serial.read();
-      logBuffer[logPos++] = val;
-      logBuffer[logPos] = 0;
-      if (val == '\n' || logPos == sizeof(logBuffer)-1) {
-        logging.send(logBuffer);
-        logPos = 0;
+      if (wifiService == WIFI_SERVICE_UPDATE) {
+        int val = Serial.read();
+        logBuffer[logPos++] = val;
+        logBuffer[logPos] = 0;
+        if (val == '\n' || logPos == sizeof(logBuffer)-1) {
+          logging.send(logBuffer);
+          logPos = 0;
+        }
+      } else if (wifiService == WIFI_SERVICE_MAVLINK_TX) {
+        mavlink_message_t msg;
+        mavlink_status_t status;
+        char val = Serial.read();
+        if (mavlink_parse_char(MAVLINK_COMM_0, val, &msg, &status)) {
+          mavlink_from_uart_counter++;
+          uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+          uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+
+          // Determine the broadcast address
+          IPAddress broadcast = WiFi.getMode() == WIFI_STA ? WiFi.broadcastIP() : apBroadcast;
+          mavlinkUDP.beginPacket(broadcast, MAVLINK_PORT_SEND);
+          mavlinkUDP.write(buf, len);
+          mavlinkUDP.endPacket();
+        }
+      }
+    }
+
+    // Check if we have MAVLink UDP data to send over UART
+    if (wifiService == WIFI_SERVICE_MAVLINK_TX) {
+      int packetSize = mavlinkUDP.parsePacket();
+      if (packetSize) {
+        uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+        mavlinkUDP.read(buf, packetSize);
+        Serial.write(buf, packetSize);
+        mavlink_to_uart_counter++;
       }
     }
 

lib/config/config.cpp

@@ -51,6 +51,7 @@ TxBackpackConfig::SetDefaults()
     m_config.ssid[0] = 0;
     m_config.password[0] = 0;
     memset(m_config.address, 0, 6);
+    m_config.wifiService = WIFI_SERVICE_UPDATE;
     m_modified = true;
     Commit();
 }
@@ -83,6 +84,12 @@ TxBackpackConfig::SetGroupAddress(const uint8_t address[6])
     m_modified = true;
 }
 
+void
+TxBackpackConfig::SetWiFiService(wifi_service_t service)
+{
+    m_config.wifiService = service;
+    m_modified = true;
+}
 #endif
 
 /////////////////////////////////////////////////////

lib/config/config.h

@@ -11,13 +11,21 @@
 #define VRX_BACKPACK_CONFIG_VERSION     3
 #define TIMER_BACKPACK_CONFIG_VERSION   3
 
+
+typedef enum {
+    WIFI_SERVICE_UPDATE,
+    WIFI_SERVICE_MAVLINK_TX,
+} wifi_service_t;
+
 #if defined(TARGET_TX_BACKPACK)
+
 typedef struct {
-    uint32_t    version;
-    bool        startWiFi;
-    char        ssid[33];
-    char        password[65];
-    uint8_t     address[6];
+    uint32_t          version;
+    bool              startWiFi;
+    char              ssid[33];
+    char              password[65];
+    uint8_t           address[6];
+    wifi_service_t    wifiService;
 } tx_backpack_config_t;
 
 class TxBackpackConfig
@@ -32,6 +40,7 @@ class TxBackpackConfig
     char    *GetSSID() { return m_config.ssid; }
     char    *GetPassword() { return m_config.password; }
     uint8_t *GetGroupAddress() { return m_config.address; }
+    wifi_service_t GetWiFiService() { return m_config.wifiService; }
 
     // Setters
     void SetStorageProvider(ELRS_EEPROM *eeprom);
@@ -40,6 +49,7 @@ class TxBackpackConfig
     void SetSSID(const char *ssid);
     void SetPassword(const char *ssid);
     void SetGroupAddress(const uint8_t address[6]);
+    void SetWiFiService(wifi_service_t service);
 
 private:
     tx_backpack_config_t    m_config;

src/Tx_main.cpp

@@ -22,13 +22,20 @@
 #include "devButton.h"
 #include "devLED.h"
 
+#if defined(MAVLINK_ENABLED)
+#include "common/mavlink.h"
+
+#endif
+
 /////////// GLOBALS ///////////
 
 uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 
 const uint8_t version[] = {LATEST_VERSION};
 
 connectionState_e connectionState = starting;
+// Assume we are in wifi update mode until we know otherwise
+wifi_service_t wifiService = WIFI_SERVICE_UPDATE;
 unsigned long rebootTime = 0;
 
 bool cacheFull = false;
@@ -65,10 +72,14 @@ void sendMSPViaEspnow(mspPacket_t *packet);
 esp_now_peer_info_t peerInfo;
 #endif
 
-void RebootIntoWifi()
+void RebootIntoWifi(wifi_service_t service = WIFI_SERVICE_UPDATE)
 {
   DBGLN("Rebooting into wifi update mode...");
   config.SetStartWiFiOnBoot(true);
+#if defined(TARGET_TX_BACKPACK)
+  // TODO it might be better to add wifi service to each type of backpack
+  config.SetWiFiService(service);
+#endif
   config.Commit();
   rebootTime = millis();
 }
@@ -236,6 +247,41 @@ void SendCachedMSP()
   }
 }
 
+// This function is only used when we're not yet in WiFi MAVLink mode - its main purpose is to detect
+// heartbeat packets and switch to WiFi MAVLink mode if we receive 3 heartbeats in 5 seconds.
+void ProcessMAVLinkFromTX(mavlink_message_t *mavlink_rx_message, mavlink_status_t *mavlink_status)
+{
+  static unsigned long heartbeatTimestamps[3] = {0, 0, 0};
+  switch (mavlink_rx_message->msgid)
+  {
+  case MAVLINK_MSG_ID_HEARTBEAT:
+  {
+    mavlink_heartbeat_t heartbeat;
+    mavlink_msg_heartbeat_decode(mavlink_rx_message, &heartbeat);
+    if (mavlink_rx_message->compid == MAV_COMP_ID_AUTOPILOT1)
+    {
+      // If we hit this line we shouldn't be in WiFi update mode but let's be certain.
+      if (connectionState != wifiUpdate)
+      {
+        unsigned long now = millis();
+        // Push the timestamp we received the heartbeat back into the array
+        for (int i = 0; i < 2; i++)
+        {
+          heartbeatTimestamps[i] = heartbeatTimestamps[i + 1];
+        }
+        heartbeatTimestamps[2] = now;
+        // If we have received 3 heartbeats in the last 5 seconds, switch to WiFi update mode
+        if (now - heartbeatTimestamps[0] < 5000)
+        {
+          RebootIntoWifi(WIFI_SERVICE_MAVLINK_TX);
+        }
+      }
+    }
+    break;
+  }
+  }
+}
+
 void SetSoftMACAddress()
 {
   if (!firmwareOptions.hasUID)
@@ -310,6 +356,12 @@ void setup()
   if (config.GetStartWiFiOnBoot())
   {
     config.SetStartWiFiOnBoot(false);
+    if (config.GetWiFiService() != WIFI_SERVICE_UPDATE)
+    {
+      // Store the intended service mode before writing default back to persistent flash
+      wifiService = config.GetWiFiService();
+      config.SetWiFiService(WIFI_SERVICE_UPDATE);
+    }
     config.Commit();
     connectionState = wifiUpdate;
     devicesTriggerEvent();
@@ -377,6 +429,13 @@ void loop()
       ProcessMSPPacketFromTX(msp.getReceivedPacket());
       msp.markPacketReceived();
     }
+    // Process MAVLink messages to switch to wifi update mode
+    mavlink_message_t mavlink_rx_message;
+    mavlink_status_t mavlink_status;
+    if (mavlink_parse_char(MAVLINK_COMM_0, c, &mavlink_rx_message, &mavlink_status))
+    {
+      ProcessMAVLinkFromTX(&mavlink_rx_message, &mavlink_status);
+    }
   }
 
   if (cacheFull && sendCached)

targets/common.ini

@@ -14,6 +14,8 @@ lib_deps =
 [common_env_data]
 build_src_filter = +<*> -<.git/> -<svn/> -<example/> -<examples/> -<test/> -<tests/> -<*.py> -<*test*.*>
 build_flags = -Wall -Iinclude
+# keep in sync with the version in the main firmware
+mavlink_lib_dep = https://github.com/mavlink/c_library_v2.git#e54a8d2e8cf7985e689ad1c8c8f37dc0800ea87b
 
 # ------------------------- COMMON ESP8285 DEFINITIONS -----------------
 [env_common_esp8285]
@@ -65,6 +67,8 @@ lib_ignore = STM32UPDATE
 build_flags =
 	${common_env_data.build_flags}
 	-D TARGET_TX_BACKPACK
+lib_deps =
+	${env.lib_deps}
 build_src_filter = ${common_env_data.build_src_filter} -<Vrx_main.cpp> -<rapidfire.*> -<rx5808.*> -<steadyview.*> -<fusion.*> -<hdzero.*> -<skyzone_msp.*> -<orqa.*> -<Timer_main.cpp>
 
 # ------------------------- COMMON RAPIDFIRE-BACKPACK DEFINITIONS -----------------

targets/txbp_esp.ini

@@ -4,9 +4,13 @@
 
 [env:ESP_TX_Backpack_via_UART]
 extends = env_common_esp8285, tx_backpack_common
+lib_deps =
+	${tx_backpack_common.lib_deps}
+	${common_env_data.mavlink_lib_dep}
 build_flags =
 	${env_common_esp8285.build_flags}
 	${tx_backpack_common.build_flags}
+	-D MAVLINK_ENABLED=1
 	-D PIN_BUTTON=0
 	-D PIN_LED=16
 upload_resetmethod = nodemcu