ExpressLRS · jlpoltrack · Apr 8, 2026
diff --git a/platformio.ini b/platformio.ini
@@ -16,3 +16,4 @@ extra_configs =
 	targets/rx5808.ini
 	targets/skyzone.ini
 	targets/mfd_crossbow.ini
+	targets/mavlink_companion.ini
diff --git a/src/Mavlink_companion_main.cpp b/src/Mavlink_companion_main.cpp
@@ -0,0 +1,209 @@
+#if defined(TARGET_MAVLINK_COMPANION)
+
+#include <Arduino.h>
+#include <esp_now.h>
+#include <esp_wifi.h>
+#include <WiFi.h>
+#include "MAVLink.h"
+#include "options.h"
+#include "logging.h"
+
+/////////// CONSTANTS ///////////
+
+#define ESPNOW_MAX_PAYLOAD 250
+
+/////////// GLOBALS ///////////
+
+uint8_t txBackpackAddress[6];
+esp_now_peer_info_t txPeerInfo;
+
+// Uplink: UART -> MAVLink message queue -> ESP-NOW
+mavlink_message_t uplinkMsgBuf[MAVLINK_BUF_SIZE];
+uint8_t           uplinkMsgCount = 0;
+unsigned long     lastUplinkFlush = 0;
+
+// Downlink: ESP-NOW -> ring buffer -> MAVLink frame -> UART
+uint8_t downlinkBuf[1024];
+volatile uint16_t downlinkHead = 0;
+volatile uint16_t downlinkTail = 0;
+
+/////////// ESP-NOW CALLBACKS ///////////
+
+// Downlink: ESP-NOW -> ring buffer (unsafe to call Serial.write from WiFi task on USB CDC)
+void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len)
+{
+  if (memcmp(mac_addr, txBackpackAddress, 6) != 0)
+  {
+    return;
+  }
+  for (int i = 0; i < data_len; i++)
+  {
+    uint16_t nextHead = (downlinkHead + 1) % sizeof(downlinkBuf);
+    if (nextHead == downlinkTail)
+    {
+      break; // buffer full, drop remaining bytes
+    }
+    downlinkBuf[downlinkHead] = data[i];
+    downlinkHead = nextHead;
+  }
+}
+
+/////////// MAC ADDRESS SETUP ///////////
+
+void SetCompanionMACAddress()
+{
+  uint8_t mac[6];
+  memcpy(mac, firmwareOptions.uid, 6);
+
+  // First byte must be even for unicast
+  mac[0] = mac[0] & ~0x01;
+  // Differentiate from TX backpack by XOR on last byte
+  mac[5] ^= 0x01;
+
+  WiFi.mode(WIFI_STA);
+  WiFi.setTxPower(WIFI_POWER_8_5dBm);
+  esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N);
+  WiFi.begin("network-name", "pass-to-network", 1);
+  WiFi.disconnect();
+
+  esp_wifi_set_mac(WIFI_IF_STA, mac);
+
+  // TX backpack address is the standard UID-derived MAC (first byte even, no XOR)
+  memcpy(txBackpackAddress, firmwareOptions.uid, 6);
+  txBackpackAddress[0] = txBackpackAddress[0] & ~0x01;
+}
+
+/////////// UPLINK FLUSH ///////////
+
+void flushUplinkBuf()
+{
+  if (uplinkMsgCount == 0)
+  {
+    return;
+  }
+
+  uint8_t buf[ESPNOW_MAX_PAYLOAD];
+  uint8_t bufLen = 0;
+
+  uint8_t tmpBuf[MAVLINK_MAX_PACKET_LEN];
+  for (uint8_t i = 0; i < uplinkMsgCount; i++)
+  {
+    uint16_t msgLen = mavlink_msg_to_send_buffer(tmpBuf, &uplinkMsgBuf[i]);
+
+    // If adding this message would overflow, send current buffer first
+    if (bufLen + msgLen > sizeof(buf) && bufLen > 0)
+    {
+      esp_now_send(txBackpackAddress, buf, bufLen);
+      bufLen = 0;
+    }
+
+    // If a single message exceeds 250 bytes, fragment it
+    if (msgLen > sizeof(buf))
+    {
+      uint16_t offset = 0;
+      while (offset < msgLen)
+      {
+        uint16_t chunkLen = min((uint16_t)(sizeof(buf)), (uint16_t)(msgLen - offset));
+        esp_now_send(txBackpackAddress, tmpBuf + offset, chunkLen);
+        offset += chunkLen;
+      }
+    }
+    else
+    {
+      memcpy(buf + bufLen, tmpBuf, msgLen);
+      bufLen += msgLen;
+    }
+  }
+
+  if (bufLen > 0)
+  {
+    esp_now_send(txBackpackAddress, buf, bufLen);
+  }
+
+  uplinkMsgCount = 0;
+}
+
+/////////// SETUP ///////////
+
+void setup()
+{
+  Serial.setRxBufferSize(4096);
+  Serial.begin(460800);
+
+  options_init();
+
+  SetCompanionMACAddress();
+
+  if (esp_now_init() != ESP_OK)
+  {
+    DBGLN("Error initializing ESP-NOW");
+    ESP.restart();
+  }
+
+  memcpy(txPeerInfo.peer_addr, txBackpackAddress, 6);
+  txPeerInfo.channel = 0;
+  txPeerInfo.encrypt = false;
+  if (esp_now_add_peer(&txPeerInfo) != ESP_OK)
+  {
+    DBGLN("ESP-NOW failed to add TX backpack peer");
+    ESP.restart();
+  }
+
+  esp_now_register_recv_cb(OnDataRecv);
+
+  DBGLN("MAVLink companion started");
+}
+
+/////////// LOOP ///////////
+
+void loop()
+{
+  uint32_t now = millis();
+
+  // Drain downlink ring buffer, frame MAVLink, write complete messages to Serial
+  {
+    mavlink_status_t dlStatus;
+    mavlink_message_t dlMsg;
+    uint8_t dlBuf[MAVLINK_MAX_PACKET_LEN];
+    while (downlinkHead != downlinkTail)
+    {
+      uint8_t c = downlinkBuf[downlinkTail];
+      downlinkTail = (downlinkTail + 1) % sizeof(downlinkBuf);
+
+      if (mavlink_frame_char(MAVLINK_COMM_1, c, &dlMsg, &dlStatus) != MAVLINK_FRAMING_INCOMPLETE)
+      {
+        uint16_t len = mavlink_msg_to_send_buffer(dlBuf, &dlMsg);
+        Serial.write(dlBuf, len);
+      }
+    }
+  }
+
+  // Parse MAVLink from UART, queue complete messages
+  {
+    mavlink_status_t ulStatus;
+    mavlink_message_t ulMsg;
+    while (Serial.available())
+    {
+      uint8_t c = Serial.read();
+
+      if (mavlink_frame_char(MAVLINK_COMM_0, c, &ulMsg, &ulStatus) != MAVLINK_FRAMING_INCOMPLETE)
+      {
+        if (uplinkMsgCount < MAVLINK_BUF_SIZE)
+        {
+          uplinkMsgBuf[uplinkMsgCount++] = ulMsg;
+        }
+      }
+    }
+  }
+
+  // Flush using same thresholds as Tx_main.cpp
+  bool thresholdHit = uplinkMsgCount >= MAVLINK_BUF_THRESHOLD;
+  bool timeoutHit   = uplinkMsgCount > 0 && (now - lastUplinkFlush) > MAVLINK_BUF_TIMEOUT;
+  if (thresholdHit || timeoutHit)
+  {
+    flushUplinkBuf();
+    lastUplinkFlush = now;
+  }
+}
+
+#endif
diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp
@@ -29,6 +29,12 @@
 /////////// GLOBALS ///////////
 
 uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+#if defined(MAVLINK_ENABLED)
+uint8_t companionAddress[6];
+uint8_t companionUplinkBuf[1024];
+volatile uint16_t companionUplinkHead = 0;
+volatile uint16_t companionUplinkTail = 0;
+#endif
 
 const uint8_t version[] = {LATEST_VERSION};
 
@@ -74,6 +80,9 @@ void sendMSPViaWiFiUDP(mspPacket_t *packet);
 // https://rntlab.com/question/espnow-peer-interface-is-invalid/
 esp_now_peer_info_t peerInfo;
 esp_now_peer_info_t bindingInfo;
+#if defined(MAVLINK_ENABLED)
+esp_now_peer_info_t companionInfo;
+#endif
 #endif
 
 void RebootIntoWifi(wifi_service_t service = WIFI_SERVICE_UPDATE)
@@ -120,6 +129,23 @@ void OnDataRecv(uint8_t * mac_addr, uint8_t *data, uint8_t data_len)
 void OnDataRecv(const uint8_t * mac_addr, const uint8_t *data, int data_len)
 #endif
 {
+#if defined(MAVLINK_ENABLED)
+  // MAVLink uplink from companion: buffer for processing in loop
+  if (memcmp(mac_addr, companionAddress, 6) == 0)
+  {
+    for (int i = 0; i < data_len; i++)
+    {
+      uint16_t nextHead = (companionUplinkHead + 1) % sizeof(companionUplinkBuf);
+      if (nextHead == companionUplinkTail)
+        break;
+      companionUplinkBuf[companionUplinkHead] = data[i];
+      companionUplinkHead = nextHead;
+    }
+    blinkLED();
+    return;
+  }
+#endif
+
   MSP recv_msp;
   DBGLN("ESP NOW DATA:");
   for(int i = 0; i < data_len; i++)
@@ -307,6 +333,60 @@ void sendMSPViaWiFiUDP(mspPacket_t *packet)
   SendTxBackpackTelemetryViaUDP(dataOutput, packetSize);
 }
 
+#if defined(MAVLINK_ENABLED)
+void sendMAVLinkViaEspnow()
+{
+  uint8_t count = mavlink.GetQueuedMsgCount();
+  if (count == 0)
+  {
+    return;
+  }
+
+  mavlink_message_t *msgQueue = mavlink.GetQueuedMsgs();
+  uint8_t buf[250];
+  uint8_t bufLen = 0;
+
+  for (uint8_t i = 0; i < count; i++)
+  {
+    uint8_t tmpBuf[MAVLINK_MAX_PACKET_LEN];
+    uint16_t msgLen = mavlink_msg_to_send_buffer(tmpBuf, &msgQueue[i]);
+
+    // If adding this message would overflow, send current buffer first
+    if (bufLen + msgLen > sizeof(buf) && bufLen > 0)
+    {
+      esp_now_send(companionAddress, buf, bufLen);
+      bufLen = 0;
+    }
+
+    // If a single message exceeds 250 bytes, fragment it
+    if (msgLen > sizeof(buf))
+    {
+      uint16_t offset = 0;
+      while (offset < msgLen)
+      {
+        uint16_t chunkLen = min((uint16_t)(sizeof(buf)), (uint16_t)(msgLen - offset));
+        esp_now_send(companionAddress, tmpBuf + offset, chunkLen);
+        offset += chunkLen;
+      }
+    }
+    else
+    {
+      memcpy(buf + bufLen, tmpBuf, msgLen);
+      bufLen += msgLen;
+    }
+  }
+
+  // Send any remaining bytes
+  if (bufLen > 0)
+  {
+    esp_now_send(companionAddress, buf, bufLen);
+  }
+
+  mavlink.ResetQueuedMsgCount();
+  blinkLED();
+}
+#endif
+
 void SendCachedMSP()
 {
   if (!cacheFull)
@@ -344,9 +424,9 @@ void SetSoftMACAddress()
 
   WiFi.mode(WIFI_STA);
   #if defined(PLATFORM_ESP8266)
-    WiFi.setOutputPower(20.5);
+    WiFi.setOutputPower(10);
   #elif defined(PLATFORM_ESP32)
-    WiFi.setTxPower(WIFI_POWER_19_5dBm);
+    WiFi.setTxPower(WIFI_POWER_8_5dBm);
     esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR);
   #endif
   WiFi.begin("network-name", "pass-to-network", 1);
@@ -442,6 +522,23 @@ void setup()
       }
     #endif
 
+    #if defined(MAVLINK_ENABLED)
+      // Add MAVLink companion as a unicast peer (MAC = binding UID with last byte XOR 0x01)
+      memcpy(companionAddress, firmwareOptions.uid, 6);
+      companionAddress[5] ^= 0x01;
+      #if defined(PLATFORM_ESP8266)
+        esp_now_add_peer(companionAddress, ESP_NOW_ROLE_COMBO, 1, NULL, 0);
+      #elif defined(PLATFORM_ESP32)
+        memcpy(companionInfo.peer_addr, companionAddress, 6);
+        companionInfo.channel = 0;
+        companionInfo.encrypt = false;
+        if (esp_now_add_peer(&companionInfo) != ESP_OK)
+        {
+          DBGLN("ESP-NOW failed to add companion peer");
+        }
+      #endif
+    #endif
+
     esp_now_register_recv_cb(OnDataRecv);
   }
 
@@ -489,4 +586,27 @@ void loop()
     SendCachedMSP();
     sendCached = false;
   }
+
+#if defined(MAVLINK_ENABLED)
+  // Drain companion uplink ring buffer and frame MAVLink to TX module UART
+  while (companionUplinkHead != companionUplinkTail)
+  {
+    uint8_t c = companionUplinkBuf[companionUplinkTail];
+    companionUplinkTail = (companionUplinkTail + 1) % sizeof(companionUplinkBuf);
+    mavlink.ProcessMAVLinkFromGCS(&c, 1);
+  }
+
+  // Flush buffered MAVLink frames via ESP-NOW to companion (only when ESP-NOW is active, not WiFi mode)
+  if (connectionState == running)
+  {
+    static unsigned long lastMavlinkFlush = 0;
+    bool thresholdHit = mavlink.GetQueuedMsgCount() >= MAVLINK_BUF_THRESHOLD;
+    bool timeoutHit = mavlink.GetQueuedMsgCount() > 0 && (now - lastMavlinkFlush) > MAVLINK_BUF_TIMEOUT;
+    if (thresholdHit || timeoutHit)
+    {
+      sendMAVLinkViaEspnow();
+      lastMavlinkFlush = now;
+    }
+  }
+#endif
 }