From 26ace5e91e17b0eed2ba09126d7ab31cabec429a Mon Sep 17 00:00:00 2001 From: MUSTARDTIGER Date: Sun, 23 Feb 2025 22:22:52 -0800 Subject: [PATCH 1/2] Make MAVLink WiFi mode also emit ESPNOW CRSF GPS frames (which should be enough to make antenna trackers work) # Conflicts: # src/Tx_main.cpp --- include/common.h | 2 ++ lib/CRSF/CRSF.cpp | 15 +++++++++ lib/CRSF/CRSF.h | 8 +++++ lib/CrsfProtocol/crsf_protocol.h | 56 +++++++++++++++++++++++++++++--- lib/ESPNOW/ESPNOW_Helpers.cpp | 39 ++++++++++++++++++++++ lib/ESPNOW/ESPNOW_Helpers.h | 7 ++++ lib/MAVLink/MAVLink.cpp | 34 +++++++++++++++++++ src/Tx_main.cpp | 45 +++++-------------------- src/Vrx_main.cpp | 24 ++------------ src/module_base.cpp | 6 ++-- targets/common.ini | 20 ++++++------ 11 files changed, 181 insertions(+), 75 deletions(-) create mode 100644 lib/CRSF/CRSF.cpp create mode 100644 lib/CRSF/CRSF.h create mode 100644 lib/ESPNOW/ESPNOW_Helpers.cpp create mode 100644 lib/ESPNOW/ESPNOW_Helpers.h diff --git a/include/common.h b/include/common.h index 8818ade9..93eace2b 100644 --- a/include/common.h +++ b/include/common.h @@ -1,4 +1,5 @@ #pragma once +#include typedef enum { @@ -11,3 +12,4 @@ typedef enum extern connectionState_e connectionState; extern unsigned long bindingStart; +static const uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; \ No newline at end of file diff --git a/lib/CRSF/CRSF.cpp b/lib/CRSF/CRSF.cpp new file mode 100644 index 00000000..9c847b01 --- /dev/null +++ b/lib/CRSF/CRSF.cpp @@ -0,0 +1,15 @@ +#include "CRSF.h" +#include + +GENERIC_CRC8 crsf_crc(CRSF_CRC_POLY); + +void CRSF::SetHeaderAndCrc(uint8_t *frame, crsf_frame_type_e frameType, uint8_t frameSize, crsf_addr_e destAddr) +{ + auto *header = (crsf_header_t *)frame; + header->sync_byte = destAddr; + header->frame_size = frameSize; + header->type = frameType; + + uint8_t crc = crsf_crc.calc(&frame[CRSF_FRAME_NOT_COUNTED_BYTES], frameSize - 1, 0); + frame[frameSize + CRSF_FRAME_NOT_COUNTED_BYTES - 1] = crc; +} \ No newline at end of file diff --git a/lib/CRSF/CRSF.h b/lib/CRSF/CRSF.h new file mode 100644 index 00000000..8205e3cb --- /dev/null +++ b/lib/CRSF/CRSF.h @@ -0,0 +1,8 @@ +#include +#include "crsf_protocol.h" + +class CRSF { +public: + static void SetHeaderAndCrc(uint8_t *frame, crsf_frame_type_e frameType, uint8_t frameSize, crsf_addr_e destAddr); +}; + diff --git a/lib/CrsfProtocol/crsf_protocol.h b/lib/CrsfProtocol/crsf_protocol.h index 6eca978a..e16da30c 100644 --- a/lib/CrsfProtocol/crsf_protocol.h +++ b/lib/CrsfProtocol/crsf_protocol.h @@ -2,10 +2,7 @@ #define CRSF_CRC_POLY 0xd5 #define CRSF_SYNC_BYTE 0xc8 - -#define CRSF_FRAMETYPE_GPS 0x02 -#define CRSF_FRAMETYPE_LINK_STATISTICS 0x14 -#define CRSF_FRAMETYPE_BATTERY_SENSOR 0x08 +#define CRSF_FRAME_NOT_COUNTED_BYTES 2 #define CRSF_CHANNEL_VALUE_1000 191 #define CRSF_CHANNEL_VALUE_MID 992 @@ -23,6 +20,57 @@ typedef struct crsf_header_s uint8_t type; // from crsf_frame_type_e } PACKED crsf_header_t; +typedef enum : uint8_t +{ + CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_VARIO = 0x07, + CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_BARO_ALTITUDE = 0x09, + CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, + CRSF_FRAMETYPE_OPENTX_SYNC = 0x10, + CRSF_FRAMETYPE_RADIO_ID = 0x3A, + CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, + CRSF_FRAMETYPE_ATTITUDE = 0x1E, + CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, + // Extended Header Frames, range: 0x28 to 0x96 + CRSF_FRAMETYPE_DEVICE_PING = 0x28, + CRSF_FRAMETYPE_DEVICE_INFO = 0x29, + CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B, + CRSF_FRAMETYPE_PARAMETER_READ = 0x2C, + CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D, + + //CRSF_FRAMETYPE_ELRS_STATUS = 0x2E, ELRS good/bad packet count and status flags + + CRSF_FRAMETYPE_COMMAND = 0x32, + // KISS frames + CRSF_FRAMETYPE_KISS_REQ = 0x78, + CRSF_FRAMETYPE_KISS_RESP = 0x79, + // MSP commands + CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command + CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary + CRSF_FRAMETYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit) + // Ardupilot frames + CRSF_FRAMETYPE_ARDUPILOT_RESP = 0x80, +} crsf_frame_type_e; + +typedef enum : uint8_t +{ + CRSF_ADDRESS_BROADCAST = 0x00, + CRSF_ADDRESS_USB = 0x10, + CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80, + CRSF_ADDRESS_RESERVED1 = 0x8A, + CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, + CRSF_ADDRESS_GPS = 0xC2, + CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, + CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8, + CRSF_ADDRESS_RESERVED2 = 0xCA, + CRSF_ADDRESS_RACE_TAG = 0xCC, + CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, + CRSF_ADDRESS_CRSF_RECEIVER = 0xEC, + CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE, + CRSF_ADDRESS_ELRS_LUA = 0xEF +} crsf_addr_e; + #define CRSF_MK_FRAME_T(payload) struct payload##_frame_s { crsf_header_t h; payload p; uint8_t crc; } PACKED typedef struct crsf_sensor_gps_s { diff --git a/lib/ESPNOW/ESPNOW_Helpers.cpp b/lib/ESPNOW/ESPNOW_Helpers.cpp new file mode 100644 index 00000000..352f528e --- /dev/null +++ b/lib/ESPNOW/ESPNOW_Helpers.cpp @@ -0,0 +1,39 @@ +#include "ESPNOW_Helpers.h" +#include +#include +#include "devLED.h" +#if defined(PLATFORM_ESP8266) + #include +#elif defined(PLATFORM_ESP32) + #include +#endif + +extern MSP msp; +extern connectionState_e connectionState; // from Vrx_main.cpp + +void ESPNOW::sendMSPViaEspnow(mspPacket_t *packet) +{ + // Do not send while in binding mode. The currently used firmwareOptions.uid may be garbage. + if (connectionState == binding) + return; + + uint8_t packetSize = msp.getTotalPacketSize(packet); + uint8_t nowDataOutput[packetSize]; + + uint8_t result = msp.convertToByteArray(packet, nowDataOutput); + + if (!result) + { + // packet could not be converted to array, bail out + return; + } + if (packet->function == MSP_ELRS_BIND) + { + esp_now_send((uint8_t*)bindingAddress, (uint8_t *) &nowDataOutput, packetSize); // Send Bind packet with the broadcast address + } + else + { + esp_now_send(firmwareOptions.uid, (uint8_t *) &nowDataOutput, packetSize); + } + blinkLED(); +} \ No newline at end of file diff --git a/lib/ESPNOW/ESPNOW_Helpers.h b/lib/ESPNOW/ESPNOW_Helpers.h new file mode 100644 index 00000000..48768280 --- /dev/null +++ b/lib/ESPNOW/ESPNOW_Helpers.h @@ -0,0 +1,7 @@ +#include "msp.h" +#include "msptypes.h" + +class ESPNOW { +public: + static void sendMSPViaEspnow(mspPacket_t *packet); +}; \ No newline at end of file diff --git a/lib/MAVLink/MAVLink.cpp b/lib/MAVLink/MAVLink.cpp index 2203ce29..56a536af 100644 --- a/lib/MAVLink/MAVLink.cpp +++ b/lib/MAVLink/MAVLink.cpp @@ -2,6 +2,11 @@ #include #include "MAVLink.h" #include +#include +#include +#include +#include +#include void MAVLink::ProcessMAVLinkFromTX(uint8_t c) @@ -40,6 +45,35 @@ MAVLink::ProcessMAVLinkFromTX(uint8_t c) mavlink_to_gcs_buf[mavlink_to_gcs_buf_count] = msg; mavlink_to_gcs_buf_count++; mavlink_stats.packets_downlink++; + + // Look for GPS packets - convert them to CRSF + if (msg.msgid == MAVLINK_MSG_ID_GPS_RAW_INT) + { + mavlink_gps_raw_int_t gps_int; + mavlink_msg_gps_raw_int_decode(&msg, &gps_int); + CRSF_MK_FRAME_T(crsf_sensor_gps_t) crsfgps = {0}; + + crsfgps.p.speed = htobe16(gps_int.vel * 36 / 100); + crsfgps.p.lat = htobe32(gps_int.lat); + crsfgps.p.lon = htobe32(gps_int.lon); + crsfgps.p.heading = htobe16(gps_int.cog); + crsfgps.p.satcnt = gps_int.satellites_visible; + crsfgps.p.altitude = htobe16(gps_int.alt / 1000 + 1000); + + CRSF::SetHeaderAndCrc((uint8_t *)&crsfgps, CRSF_FRAMETYPE_GPS, sizeof(crsf_sensor_gps_t), CRSF_ADDRESS_CRSF_TRANSMITTER); + + // Wrap in MSP + mspPacket_t packet; + packet.reset(); + packet.makeCommand(); + packet.function = MSP_ELRS_BACKPACK_CRSF_TLM; + for (size_t i = 0; i < sizeof(crsfgps); i++) + { + packet.addByte(((uint8_t *)&crsfgps)[i]); + } + // Send it out ESPNOW + ESPNOW::sendMSPViaEspnow(&packet); + } } } diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index c37911fc..6ea5c20f 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -11,6 +11,7 @@ #include "msp.h" #include "msptypes.h" +#include "ESPNOW_Helpers.h" #include "logging.h" #include "config.h" #include "common.h" @@ -28,8 +29,6 @@ /////////// GLOBALS /////////// -uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - const uint8_t version[] = {LATEST_VERSION}; connectionState_e connectionState = starting; @@ -63,7 +62,6 @@ MAVLink mavlink; /////////// FUNCTION DEFS /////////// -void sendMSPViaEspnow(mspPacket_t *packet); void sendMSPViaWiFiUDP(mspPacket_t *packet); ///////////////////////////////////// @@ -198,12 +196,12 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) cachedVTXPacket = *packet; cacheFull = true; // transparently forward MSP packets via espnow to any subscribers - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; case MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE: DBGLN("Processing MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE..."); - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; case MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE: @@ -220,7 +218,7 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) DBGLN("Processing MSP_ELRS_BACKPACK_SET_HEAD_TRACKING..."); cachedHTPacket = *packet; cacheFull = true; - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; case MSP_ELRS_BACKPACK_CRSF_TLM: @@ -231,7 +229,7 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) } if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF) { - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); } break; @@ -258,41 +256,16 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) rebootTime = millis(); // restart to set SetSoftMACAddress return; } - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; default: // transparently forward MSP packets via espnow to any subscribers - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; } } -void sendMSPViaEspnow(mspPacket_t *packet) -{ - uint8_t packetSize = msp.getTotalPacketSize(packet); - uint8_t nowDataOutput[packetSize]; - - uint8_t result = msp.convertToByteArray(packet, nowDataOutput); - - if (!result) - { - // packet could not be converted to array, bail out - return; - } - - if (packet->function == MSP_ELRS_BIND) - { - esp_now_send(bindingAddress, (uint8_t *) &nowDataOutput, packetSize); // Send Bind packet with the broadcast address - } - else - { - esp_now_send(firmwareOptions.uid, (uint8_t *) &nowDataOutput, packetSize); - } - - blinkLED(); -} - void sendMSPViaWiFiUDP(mspPacket_t *packet) { uint8_t packetSize = msp.getTotalPacketSize(packet); @@ -317,11 +290,11 @@ void SendCachedMSP() if (cachedVTXPacket.type != MSP_PACKET_UNKNOWN) { - sendMSPViaEspnow(&cachedVTXPacket); + ESPNOW::sendMSPViaEspnow(&cachedVTXPacket); } if (cachedHTPacket.type != MSP_PACKET_UNKNOWN) { - sendMSPViaEspnow(&cachedHTPacket); + ESPNOW::sendMSPViaEspnow(&cachedHTPacket); } } diff --git a/src/Vrx_main.cpp b/src/Vrx_main.cpp index e9921275..f7438fb4 100644 --- a/src/Vrx_main.cpp +++ b/src/Vrx_main.cpp @@ -12,6 +12,7 @@ #include "msp.h" #include "msptypes.h" +#include "ESPNOW_Helpers.h" #include "logging.h" #include "helpers.h" #include "common.h" @@ -129,7 +130,6 @@ VrxBackpackConfig config; /////////// FUNCTION DEFS /////////// void ProcessMSPPacket(mspPacket_t *packet); -void sendMSPViaEspnow(mspPacket_t *packet); void resetBootCounter(); void SetupEspNow(); @@ -349,30 +349,10 @@ void RequestVTXPacket() packet.addByte(0); // empty byte blinkLED(); - sendMSPViaEspnow(&packet); + ESPNOW::sendMSPViaEspnow(&packet); #endif } -void sendMSPViaEspnow(mspPacket_t *packet) -{ - // Do not send while in binding mode. The currently used firmwareOptions.uid may be garbage. - if (connectionState == binding) - return; - - uint8_t packetSize = msp.getTotalPacketSize(packet); - uint8_t nowDataOutput[packetSize]; - - uint8_t result = msp.convertToByteArray(packet, nowDataOutput); - - if (!result) - { - // packet could not be converted to array, bail out - return; - } - - esp_now_send(firmwareOptions.uid, (uint8_t *) &nowDataOutput, packetSize); -} - void resetBootCounter() { config.SetBootCount(0); diff --git a/src/module_base.cpp b/src/module_base.cpp index 989eb4e3..a4563a81 100644 --- a/src/module_base.cpp +++ b/src/module_base.cpp @@ -6,6 +6,7 @@ #include "msptypes.h" #include "logging.h" #include +#include "ESPNOW_Helpers.h" #if defined(HAS_HEADTRACKING) #include "devHeadTracker.h" @@ -17,7 +18,6 @@ void RebootIntoWifi(wifi_service_t service = WIFI_SERVICE_UPDATE); bool BindingExpired(uint32_t now); extern uint8_t backpackVersion[]; extern bool headTrackingEnabled; -void sendMSPViaEspnow(mspPacket_t *packet); void ModuleBase::Init() @@ -84,7 +84,7 @@ ModuleBase::Loop(uint32_t now) packet.addByte(roll>> 8); packet.addByte(tilt & 0xFF); // rotating about pitch is roll axis packet.addByte(tilt >> 8); - sendMSPViaEspnow(&packet); + ESPNOW::sendMSPViaEspnow(&packet); lastSend = now; // Serial.printf("%d, %d, %d\r\n",pan, tilt, roll); } @@ -157,7 +157,7 @@ MSPModuleBase::Loop(uint32_t now) ptrChannelData[0] = packet->payload[0] + (packet->payload[1] << 8); ptrChannelData[1] = packet->payload[2] + (packet->payload[3] << 8); ptrChannelData[2] = packet->payload[4] + (packet->payload[5] << 8); - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); } msp.markPacketReceived(); } diff --git a/targets/common.ini b/targets/common.ini index 282794ed..0889bb30 100644 --- a/targets/common.ini +++ b/targets/common.ini @@ -92,7 +92,7 @@ build_src_filter = - - - - - + - - - - @@ -113,7 +113,7 @@ build_src_filter = ; - - - - - + - - - - @@ -134,7 +134,7 @@ build_src_filter = - ; - - - - + - - - - @@ -155,7 +155,7 @@ build_src_filter = - - ; - - - + - - - - @@ -176,7 +176,7 @@ build_src_filter = - - - - ; - + ; - - - - @@ -197,7 +197,7 @@ build_src_filter = - - - - - + - ; - - - @@ -218,7 +218,7 @@ build_src_filter = - - - - - + - - ; - - @@ -239,7 +239,7 @@ build_src_filter = - - - - - + - - - ; - @@ -259,7 +259,7 @@ build_src_filter = - - - - - + - - - - @@ -280,7 +280,7 @@ build_src_filter = - - - - - + - - - - From d774aeec3a0d8eecbd40ce5e166c42beabb379f0 Mon Sep 17 00:00:00 2001 From: MUSTARDTIGERFPV Date: Tue, 28 Jul 2026 23:53:01 +0200 Subject: [PATCH 2/2] MAVLink: fix GPS to ESPNOW frames never leaving the backpack ESP-NOW was only ever initialised in the non-WiFi branch of setup(), but BACKPACK_TELEM_MODE_WIFI sets StartWiFiOnBoot, so in MAVLink WiFi mode esp_now_send() ran against an uninitialised stack and returned ESP_ERR_ESPNOW_NOT_INIT. The return value was discarded, so the whole path looked healthy while nothing went out on air. Split the UID resolution out of SetSoftMACAddress() and move ESP-NOW bring-up into StartEspNow(), called from devWIFI once WiFi has settled -- it cannot run in setup() because startWiFi() takes the radio down again. Peers are re-bound if WiFi changes interface, since a station that fails to associate falls back to AP mode. Bind peers to the live interface on ESP32 (sending on the down STA interface in AP mode fails with ESP_ERR_ESPNOW_IF) and to the current channel on ESP8266 rather than a hardcoded 1. Log esp_now_send() failures so this cannot go silent again. Also fix the GPS frame itself: frame_size counts the payload plus type and crc, and crsf_sensor_gps_t was missing PACKED so sizeof() was 16 rather than the 15 bytes on the wire. Between them the crc byte was written over satcnt, which the AAT uses to decide when home is set. Use CRSF_FRAME_SIZE() and send telemetry with the 0xC8 sync byte that AatModule::onCrsfPacketIn expects. --- lib/CrsfProtocol/crsf_protocol.h | 8 +- lib/ESPNOW/ESPNOW_Helpers.cpp | 17 +++-- lib/MAVLink/MAVLink.cpp | 5 +- lib/WIFI/devWIFI.cpp | 10 +++ src/Tx_main.cpp | 124 ++++++++++++++++++++++++------- 5 files changed, 129 insertions(+), 35 deletions(-) diff --git a/lib/CrsfProtocol/crsf_protocol.h b/lib/CrsfProtocol/crsf_protocol.h index e16da30c..452def0c 100644 --- a/lib/CrsfProtocol/crsf_protocol.h +++ b/lib/CrsfProtocol/crsf_protocol.h @@ -73,6 +73,12 @@ typedef enum : uint8_t #define CRSF_MK_FRAME_T(payload) struct payload##_frame_s { crsf_header_t h; payload p; uint8_t crc; } PACKED +// frame_size counts everything after itself, i.e. the payload plus type and crc +#define CRSF_FRAME_SIZE(payload_size) ((payload_size) + 2) + +// Must be PACKED: without it the compiler adds a tail pad byte after satcnt, so +// sizeof() is 16 rather than the 15 bytes actually on the wire, and the crc member +// sits one byte past where CRSF puts it. typedef struct crsf_sensor_gps_s { int32_t lat; // degrees * 1e7 int32_t lon; // degrees * 1e7 @@ -80,7 +86,7 @@ typedef struct crsf_sensor_gps_s { uint16_t heading; // big-endian degrees * 10 uint16_t altitude; // big endian meters + 1000 uint8_t satcnt; // number of satellites -} crsf_sensor_gps_t; +} PACKED crsf_sensor_gps_t; typedef CRSF_MK_FRAME_T(crsf_sensor_gps_t) crsf_packet_gps_t; #if !defined(__linux__) diff --git a/lib/ESPNOW/ESPNOW_Helpers.cpp b/lib/ESPNOW/ESPNOW_Helpers.cpp index 352f528e..9555c57f 100644 --- a/lib/ESPNOW/ESPNOW_Helpers.cpp +++ b/lib/ESPNOW/ESPNOW_Helpers.cpp @@ -2,6 +2,7 @@ #include #include #include "devLED.h" +#include "logging.h" #if defined(PLATFORM_ESP8266) #include #elif defined(PLATFORM_ESP32) @@ -27,13 +28,17 @@ void ESPNOW::sendMSPViaEspnow(mspPacket_t *packet) // packet could not be converted to array, bail out return; } - if (packet->function == MSP_ELRS_BIND) - { - esp_now_send((uint8_t*)bindingAddress, (uint8_t *) &nowDataOutput, packetSize); // Send Bind packet with the broadcast address - } - else + // Send Bind packets with the broadcast address, everything else to the peer + const uint8_t *dest = (packet->function == MSP_ELRS_BIND) ? bindingAddress : firmwareOptions.uid; + + // Don't swallow the error. If ESP-NOW was never initialised (as happens when the + // backpack boots straight into WiFi mode) every send fails here, and without this + // log the whole path looks like it is working while nothing goes out on air. + int err = esp_now_send((uint8_t *)dest, (uint8_t *)&nowDataOutput, packetSize); + if (err != 0) { - esp_now_send(firmwareOptions.uid, (uint8_t *) &nowDataOutput, packetSize); + DBGLN("esp_now_send failed (%d) for function 0x%x", err, packet->function); + return; } blinkLED(); } \ No newline at end of file diff --git a/lib/MAVLink/MAVLink.cpp b/lib/MAVLink/MAVLink.cpp index 56a536af..b8ab4590 100644 --- a/lib/MAVLink/MAVLink.cpp +++ b/lib/MAVLink/MAVLink.cpp @@ -60,7 +60,10 @@ MAVLink::ProcessMAVLinkFromTX(uint8_t c) crsfgps.p.satcnt = gps_int.satellites_visible; crsfgps.p.altitude = htobe16(gps_int.alt / 1000 + 1000); - CRSF::SetHeaderAndCrc((uint8_t *)&crsfgps, CRSF_FRAMETYPE_GPS, sizeof(crsf_sensor_gps_t), CRSF_ADDRESS_CRSF_TRANSMITTER); + // CRSF_ADDRESS_FLIGHT_CONTROLLER is CRSF_SYNC_BYTE, which is what telemetry + // frames carry and what AatModule::onCrsfPacketIn checks for. + CRSF::SetHeaderAndCrc((uint8_t *)&crsfgps, CRSF_FRAMETYPE_GPS, + CRSF_FRAME_SIZE(sizeof(crsf_sensor_gps_t)), CRSF_ADDRESS_FLIGHT_CONTROLLER); // Wrap in MSP mspPacket_t packet; diff --git a/lib/WIFI/devWIFI.cpp b/lib/WIFI/devWIFI.cpp index 26b4013f..da7b49f1 100644 --- a/lib/WIFI/devWIFI.cpp +++ b/lib/WIFI/devWIFI.cpp @@ -51,6 +51,7 @@ extern TxBackpackConfig config; extern wifi_service_t wifiService; #if defined(MAVLINK_ENABLED) extern MAVLink mavlink; +extern bool StartEspNow(); // Tx_main.cpp #endif #elif defined(TARGET_TIMER_BACKPACK) extern TimerBackpackConfig config; @@ -880,6 +881,15 @@ static void HandleWebUpdate() #if defined(MAVLINK_ENABLED) if (wifiService == WIFI_SERVICE_MAVLINK_TX) { + // Once WiFi has settled on an interface, bring ESP-NOW up alongside it. This is + // what lets MAVLink WiFi mode still emit CRSF GPS frames to ESP-NOW peers such as + // antenna trackers. Re-checked every loop because a station that fails to + // associate falls back to AP mode, which moves the interface out from under us. + if (wifiMode == WIFI_AP || (wifiMode == WIFI_STA && status == WL_CONNECTED)) + { + StartEspNow(); + } + // Dump the mavlink_to_gcs_buf to the GCS static unsigned long last_mavlink_to_gcs_dump = 0; diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index 6ea5c20f..21eecdc7 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -38,6 +38,7 @@ unsigned long rebootTime = 0; bool cacheFull = false; bool sendCached = false; +bool espnowStarted = false; device_t *ui_devices[] = { #ifdef PIN_LED @@ -298,7 +299,9 @@ void SendCachedMSP() } } -void SetSoftMACAddress() +// Resolve the UID we peer with over ESP-NOW. Does not touch the radio, so it is +// safe to call while WiFi is already up. +void ResolveUID() { if (!firmwareOptions.hasUID) { @@ -314,6 +317,11 @@ void SetSoftMACAddress() // MAC address can only be set with unicast, so first byte must be even, not odd firmwareOptions.uid[0] = firmwareOptions.uid[0] & ~0x01; +} + +void SetSoftMACAddress() +{ + ResolveUID(); WiFi.mode(WIFI_STA); #if defined(PLATFORM_ESP8266) @@ -333,6 +341,88 @@ void SetSoftMACAddress() #endif } +// Bring up ESP-NOW on whatever interface WiFi is currently using. Safe to call +// repeatedly: it only does work the first time, or after WiFi has moved to a +// different interface/channel and the peers need re-binding. +// +// Caveat for WiFi mode: ESP-NOW has to share the radio's single channel with WiFi. +// In AP mode we own the channel, so peers running plain ESP-NOW (channel 1) hear us. +// In STA mode we are pinned to the home router's channel, so unless that router is +// also on channel 1 an ESP-NOW peer such as an antenna tracker will not receive +// anything. Fixing that properly needs the peers to follow us, which is out of +// scope here -- AP mode is the usual MAVLink-over-WiFi setup. +bool StartEspNow() +{ + #if defined(PLATFORM_ESP8266) + // Peers must sit on the channel the radio is actually on. In WiFi mode that is + // whatever the AP/station settled on, not the ESP-NOW default of 1. + uint8_t channel = wifi_get_channel(); + if (channel == 0) + { + channel = 1; + } + const uint8_t boundTo = channel; + #elif defined(PLATFORM_ESP32) + // When WiFi is running as a soft-AP the station interface is down, and sending on + // it fails with ESP_ERR_ESPNOW_IF. Bind the peers to the interface that is up. + const wifi_interface_t ifidx = ((int)WiFi.getMode() & (int)WIFI_MODE_AP) ? WIFI_IF_AP : WIFI_IF_STA; + const uint8_t boundTo = (uint8_t)ifidx; + #else + const uint8_t boundTo = 0; + #endif + + static uint8_t espnowBoundTo = 0; + if (espnowStarted) + { + if (espnowBoundTo == boundTo) + { + return true; + } + // WiFi moved (e.g. a station that could not associate fell back to AP mode). + // Tear ESP-NOW down so the peers get re-added against the interface that is up. + DBGLN("WiFi interface changed, restarting ESP-NOW"); + esp_now_deinit(); + espnowStarted = false; + } + + if (esp_now_init() != 0) + { + DBGLN("Error initializing ESP-NOW"); + return false; + } + + #if defined(PLATFORM_ESP8266) + esp_now_set_self_role(ESP_NOW_ROLE_COMBO); + esp_now_add_peer(firmwareOptions.uid, ESP_NOW_ROLE_COMBO, channel, NULL, 0); + esp_now_add_peer((uint8_t *)bindingAddress, ESP_NOW_ROLE_COMBO, channel, NULL, 0); + #elif defined(PLATFORM_ESP32) + memcpy(peerInfo.peer_addr, firmwareOptions.uid, 6); + peerInfo.channel = 0; // 0 == use the current channel + peerInfo.encrypt = false; + peerInfo.ifidx = ifidx; + if (esp_now_add_peer(&peerInfo) != ESP_OK) + { + DBGLN("ESP-NOW failed to add peer"); + return false; + } + memcpy(bindingInfo.peer_addr, bindingAddress, 6); + bindingInfo.channel = 0; + bindingInfo.encrypt = false; + bindingInfo.ifidx = ifidx; + if (esp_now_add_peer(&bindingInfo) != ESP_OK) + { + DBGLN("ESP-NOW failed to add binding peer"); + return false; + } + #endif + + esp_now_register_recv_cb(OnDataRecv); + espnowStarted = true; + espnowBoundTo = boundTo; + DBGLN("ESP-NOW started"); + return true; +} + #if defined(PLATFORM_ESP8266) // Called from core's user_rf_pre_init() function (which is called by SDK) before setup() RF_PRE_INIT() @@ -379,6 +469,11 @@ void setup() config.SetStartWiFiOnBoot(false); config.Commit(); } + // ESP-NOW cannot be started here: devWIFI takes the radio down and back up as it + // brings the AP/station online, which would tear it straight back down. It is + // started from devWIFI once WiFi has settled. Resolve the UID now so the peer + // address (and the MAVLink AP SSID, which is derived from it) is valid. + ResolveUID(); connectionState = wifiUpdate; devicesTriggerEvent(); } @@ -386,35 +481,10 @@ void setup() { SetSoftMACAddress(); - if (esp_now_init() != 0) + if (!StartEspNow()) { - DBGLN("Error initializing ESP-NOW"); rebootTime = millis(); } - - #if defined(PLATFORM_ESP8266) - esp_now_set_self_role(ESP_NOW_ROLE_COMBO); - esp_now_add_peer(firmwareOptions.uid, ESP_NOW_ROLE_COMBO, 1, NULL, 0); - #elif defined(PLATFORM_ESP32) - memcpy(peerInfo.peer_addr, firmwareOptions.uid, 6); - peerInfo.channel = 0; - peerInfo.encrypt = false; - if (esp_now_add_peer(&peerInfo) != ESP_OK) - { - DBGLN("ESP-NOW failed to add peer"); - return; - } - memcpy(bindingInfo.peer_addr, bindingAddress, 6); - bindingInfo.channel = 0; - bindingInfo.encrypt = false; - if (esp_now_add_peer(&bindingInfo) != ESP_OK) - { - DBGLN("ESP-NOW failed to add binding peer"); - return; - } - #endif - - esp_now_register_recv_cb(OnDataRecv); } devicesStart();