Make MAVLink WiFi mode also emit ESPNOW CRSF GPS frames (which should be enough to make antenna trackers work)

Author: MUSTARDTIGERFPV

include/common.h

@@ -11,3 +11,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 PACKED __attribute__((packed))
 
@@ -19,6 +16,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 {

lib/ESPNOW/ESPNOW_Helpers.cpp

@@ -0,0 +1,35 @@
+#include "ESPNOW_Helpers.h"
+#include "espnow.h"
+#include <options.h>
+#include <common.h>
+#include "devLED.h"
+
+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();
+}

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,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);
+        }
     }
 }
 

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;
@@ -61,11 +60,6 @@ mspPacket_t cachedHTPacket;
 MAVLink mavlink;
 #endif
 
-/////////// FUNCTION DEFS ///////////
-
-void sendMSPViaEspnow(mspPacket_t *packet);
-
-/////////////////////////////////////
 
 #if defined(PLATFORM_ESP32)
 // This seems to need to be global, as per this page,
@@ -200,7 +194,7 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
     DBG(""); // Extra line for serial output readability
     config.Commit();
     // delay(500); // delay may not be required
-    sendMSPViaEspnow(packet);
+    ESPNOW::sendMSPViaEspnow(packet);
     // delay(500); // delay may not be required
     rebootTime = millis(); // restart to set SetSoftMACAddress
   }
@@ -212,11 +206,11 @@ 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:
     DBGLN("Processing MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE...");
@@ -230,13 +224,13 @@ 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:
     DBGLN("Processing MSP_ELRS_BACKPACK_CRSF_TLM...");
     if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF)
     {
-      sendMSPViaEspnow(packet);
+      ESPNOW::sendMSPViaEspnow(packet);
     }
     break;
   case MSP_ELRS_BACKPACK_CONFIG:
@@ -245,36 +239,11 @@ void ProcessMSPPacketFromTX(mspPacket_t *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 SendCachedMSP()
 {
   if (!cacheFull)
@@ -285,11 +254,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);
   }
 }
 

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"
@@ -125,7 +126,6 @@ VrxBackpackConfig config;
 /////////// FUNCTION DEFS ///////////
 
 void ProcessMSPPacket(mspPacket_t *packet);
-void sendMSPViaEspnow(mspPacket_t *packet);
 void resetBootCounter();
 void SetupEspNow();
 
@@ -340,30 +340,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);