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

This reverts commit cb5c9c8.

Author: MUSTARDTIGERFPV

include/common.h

@@ -11,4 +11,3 @@ 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

@@ -2,7 +2,10 @@
 
 #define CRSF_CRC_POLY 0xd5
 #define CRSF_SYNC_BYTE 0xc8
-#define CRSF_FRAME_NOT_COUNTED_BYTES 2
+
+#define CRSF_FRAMETYPE_GPS 0x02
+#define CRSF_FRAMETYPE_LINK_STATISTICS 0x14
+#define CRSF_FRAMETYPE_BATTERY_SENSOR 0x08
 
 #define PACKED __attribute__((packed))
 
@@ -16,57 +19,6 @@ 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/CRSF/CRSF.h

@@ -2,11 +2,6 @@
 #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)
@@ -45,35 +40,6 @@ 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);
-        }
     }
 }
 

lib/CrsfProtocol/crsf_protocol.h

@@ -11,7 +11,6 @@
 
 #include "msp.h"
 #include "msptypes.h"
-#include "ESPNOW_Helpers.h"
 #include "logging.h"
 #include "config.h"
 #include "common.h"
@@ -29,6 +28,8 @@
 
 /////////// GLOBALS ///////////
 
+uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+
 const uint8_t version[] = {LATEST_VERSION};
 
 connectionState_e connectionState = starting;
@@ -60,6 +61,11 @@ 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,
@@ -194,7 +200,7 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
     DBG(""); // Extra line for serial output readability
     config.Commit();
     // delay(500); // delay may not be required
-    ESPNOW::sendMSPViaEspnow(packet);
+    sendMSPViaEspnow(packet);
     // delay(500); // delay may not be required
     rebootTime = millis(); // restart to set SetSoftMACAddress
   }
@@ -206,11 +212,11 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
     cachedVTXPacket = *packet;
     cacheFull = true;
     // transparently forward MSP packets via espnow to any subscribers
-    ESPNOW::sendMSPViaEspnow(packet);
+    sendMSPViaEspnow(packet);
     break;
   case MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE:
     DBGLN("Processing MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE...");
-    ESPNOW::sendMSPViaEspnow(packet);
+    sendMSPViaEspnow(packet);
     break;
   case MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE:
     DBGLN("Processing MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE...");
@@ -224,13 +230,13 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
     DBGLN("Processing MSP_ELRS_BACKPACK_SET_HEAD_TRACKING...");
     cachedHTPacket = *packet;
     cacheFull = true;
-    ESPNOW::sendMSPViaEspnow(packet);
+    sendMSPViaEspnow(packet);
     break;
   case MSP_ELRS_BACKPACK_CRSF_TLM:
     DBGLN("Processing MSP_ELRS_BACKPACK_CRSF_TLM...");
     if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF)
     {
-      ESPNOW::sendMSPViaEspnow(packet);
+      sendMSPViaEspnow(packet);
     }
     break;
   case MSP_ELRS_BACKPACK_CONFIG:
@@ -239,11 +245,36 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
     break;
   default:
     // transparently forward MSP packets via espnow to any subscribers
-    ESPNOW::sendMSPViaEspnow(packet);
+    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)
@@ -254,11 +285,11 @@ void SendCachedMSP()
 
   if (cachedVTXPacket.type != MSP_PACKET_UNKNOWN)
   {
-    ESPNOW::sendMSPViaEspnow(&cachedVTXPacket);
+    sendMSPViaEspnow(&cachedVTXPacket);
   }
   if (cachedHTPacket.type != MSP_PACKET_UNKNOWN)
   {
-    ESPNOW::sendMSPViaEspnow(&cachedHTPacket);
+    sendMSPViaEspnow(&cachedHTPacket);
   }
 }
 

lib/ESPNOW/ESPNOW_Helpers.cpp

@@ -12,7 +12,6 @@
 
 #include "msp.h"
 #include "msptypes.h"
-#include "ESPNOW_Helpers.h"
 #include "logging.h"
 #include "helpers.h"
 #include "common.h"
@@ -126,6 +125,7 @@ VrxBackpackConfig config;
 /////////// FUNCTION DEFS ///////////
 
 void ProcessMSPPacket(mspPacket_t *packet);
+void sendMSPViaEspnow(mspPacket_t *packet);
 void resetBootCounter();
 void SetupEspNow();
 
@@ -340,10 +340,30 @@ void RequestVTXPacket()
   packet.addByte(0);  // empty byte
 
   blinkLED();
-  ESPNOW::sendMSPViaEspnow(&packet);
+  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);