From da3dde64e864bd6d620816dfec382ed4d7ddc3d6 Mon Sep 17 00:00:00 2001 From: JLP Date: Wed, 8 Apr 2026 19:18:15 -0400 Subject: [PATCH] ESPNOW-MAVLink --- platformio.ini | 1 + src/Mavlink_companion_main.cpp | 209 +++++++++++++++++++++++++++++++++ src/Tx_main.cpp | 124 ++++++++++++++++++- targets/common.ini | 31 ++++- targets/mavlink_companion.ini | 24 ++++ 5 files changed, 386 insertions(+), 3 deletions(-) create mode 100644 src/Mavlink_companion_main.cpp create mode 100644 targets/mavlink_companion.ini diff --git a/platformio.ini b/platformio.ini index 7bad0d8b..1a710670 100644 --- 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 new file mode 100644 index 00000000..9c66f1da --- /dev/null +++ b/src/Mavlink_companion_main.cpp @@ -0,0 +1,209 @@ +#if defined(TARGET_MAVLINK_COMPANION) + +#include +#include +#include +#include +#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 index 0e38f280..ccf76444 100644 --- 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 } diff --git a/targets/common.ini b/targets/common.ini index c8b200bd..8d8e53df 100644 --- a/targets/common.ini +++ b/targets/common.ini @@ -277,4 +277,33 @@ build_src_filter = ; - lib_deps = ${env.lib_deps} - ${common_env_data.mavlink_lib_dep} \ No newline at end of file + ${common_env_data.mavlink_lib_dep} + +# ------------------------- COMMON MAVLINK-COMPANION DEFINITIONS ----------------- +[mavlink_companion_common] +build_flags = + ${common_env_data.build_flags} + -D TARGET_MAVLINK_COMPANION + -D MAVLINK_ENABLED +build_src_filter = + ${common_env_data.build_src_filter} + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + ; - +lib_deps = + ${env.lib_deps} + ${common_env_data.mavlink_lib_dep} diff --git a/targets/mavlink_companion.ini b/targets/mavlink_companion.ini new file mode 100644 index 00000000..191ee9df --- /dev/null +++ b/targets/mavlink_companion.ini @@ -0,0 +1,24 @@ +# ******************************** +# MAVLink ESP-NOW Companion +# ******************************** + +[env:MAVLink_Companion_ESP32_via_UART] +extends = env_common_esp32, mavlink_companion_common +build_flags = + ${env_common_esp32.build_flags} + ${mavlink_companion_common.build_flags} + +[env:MAVLink_Companion_ESP32C3_via_USB] +extends = env_common_esp32c3, mavlink_companion_common +build_flags = + ${env_common_esp32c3.build_flags} + ${mavlink_companion_common.build_flags} + -D ARDUINO_USB_CDC_ON_BOOT=1 + +[env:MAVLink_Companion_ESP32S3_via_USB] +extends = env_common_esp32s3, mavlink_companion_common +build_flags = + ${env_common_esp32s3.build_flags} + ${mavlink_companion_common.build_flags} + -D ARDUINO_USB_MODE=1 + -D ARDUINO_USB_CDC_ON_BOOT=1