Make MAVLink WiFi mode also emit ESPNOW CRSF GPS frames

include/common.h

@@ -1,4 +1,5 @@
 #pragma once
+#include <cstdint>
 
 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};

lib/CRSF/CRSF.cpp

@@ -0,0 +1,15 @@
+#include "CRSF.h"
+#include <crc.h>
+
+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;
+}

lib/CRSF/CRSF.h

@@ -0,0 +1,8 @@
+#include <stdint.h>
+#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);
+};
+

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,16 +20,73 @@ 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
 
+// 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
     uint16_t speed;     // big-endian km/h * 10
     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__)

lib/ESPNOW/ESPNOW_Helpers.cpp

@@ -0,0 +1,44 @@
+#include "ESPNOW_Helpers.h"
+#include <options.h>
+#include <common.h>
+#include "devLED.h"
+#include "logging.h"
+#if defined(PLATFORM_ESP8266)
+  #include <espnow.h>
+#elif defined(PLATFORM_ESP32)
+  #include <esp_now.h>
+#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;
+    }
+    // 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)
+    {
+        DBGLN("esp_now_send failed (%d) for function 0x%x", err, packet->function);
+        return;
+    }
+    blinkLED();
+}

lib/ESPNOW/ESPNOW_Helpers.h

@@ -0,0 +1,7 @@
+#include "msp.h"
+#include "msptypes.h"
+
+class ESPNOW {
+public:
+    static void sendMSPViaEspnow(mspPacket_t *packet);
+};

lib/MAVLink/MAVLink.cpp

@@ -2,6 +2,11 @@
 #include <Arduino.h>
 #include "MAVLink.h"
 #include <config.h>
+#include <crsf_protocol.h>
+#include <msp.h>
+#include <msptypes.h>
+#include <CRSF.h>
+#include <ESPNOW_Helpers.h>
 
 void
 MAVLink::ProcessMAVLinkFromTX(uint8_t c)
@@ -40,6 +45,38 @@ 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_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;
+            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);
+        }
     }
 }
 

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;