Merge pull request #211 from xNuclearSquirrel/CRSF_TLM_UDP

Make CRSF telemetry accessible over WIFI as UDP broadcast

Author: pkendall64

lib/WIFI/devWIFI.cpp

@@ -119,6 +119,58 @@ IPAddress gcsIP;
 bool gcsIPSet = false;
 #endif
 
+#if defined(TARGET_TX_BACKPACK)
+bool SendTxBackpackTelemetryViaUDP(const uint8_t *data, uint16_t size)
+{
+  if (!data || size == 0)
+  {
+    return false;
+  }
+
+#if !defined(MAVLINK_ENABLED)
+  return false;
+#else
+  if (!servicesStarted || !wifiStarted)
+  {
+    return false;
+  }
+  if (wifiService != WIFI_SERVICE_MAVLINK_TX)
+  {
+    return false;
+  }
+
+  IPAddress remote;
+  WiFiMode_t mode = WiFi.getMode();
+  if (mode == WIFI_AP || mode == WIFI_AP_STA)
+  {
+    remote = apBroadcast;
+  }
+  else if (mode == WIFI_STA)
+  {
+    remote = WiFi.broadcastIP();
+  }
+  else
+  {
+    return false;
+  }
+
+  if (!mavlinkUDP.beginPacket(remote, config.GetMavlinkSendPort()))
+  {
+    return false;
+  }
+
+  size_t sent = mavlinkUDP.write(data, size);
+  if (sent != size)
+  {
+    mavlinkUDP.endPacket();
+    return false;
+  }
+
+  return mavlinkUDP.endPacket() == 1;
+#endif
+}
+#endif
+
 /** Is this an IP? */
 static boolean isIp(String str)
 {

lib/WIFI/devWIFI.h

@@ -7,4 +7,8 @@ extern device_t WIFI_device;
 #define HAS_WIFI
 
 extern const char *VERSION;
-#endif
+
+#if defined(TARGET_TX_BACKPACK)
+bool SendTxBackpackTelemetryViaUDP(const uint8_t *data, uint16_t size);
+#endif
+#endif

src/Tx_main.cpp

@@ -64,6 +64,7 @@ MAVLink mavlink;
 /////////// FUNCTION DEFS ///////////
 
 void sendMSPViaEspnow(mspPacket_t *packet);
+void sendMSPViaWiFiUDP(mspPacket_t *packet);
 
 /////////////////////////////////////
 
@@ -224,6 +225,10 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
 
   case MSP_ELRS_BACKPACK_CRSF_TLM:
     DBGLN("Processing MSP_ELRS_BACKPACK_CRSF_TLM...");
+    if (config.GetTelemMode() == BACKPACK_TELEM_MODE_WIFI)
+    {
+      sendMSPViaWiFiUDP(packet);
+    }
     if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF)
     {
       sendMSPViaEspnow(packet);
@@ -288,6 +293,20 @@ void sendMSPViaEspnow(mspPacket_t *packet)
   blinkLED();
 }
 
+void sendMSPViaWiFiUDP(mspPacket_t *packet)
+{
+  uint8_t packetSize = msp.getTotalPacketSize(packet);
+  uint8_t dataOutput[packetSize];
+
+  uint8_t result = msp.convertToByteArray(packet, dataOutput);
+  if (!result)
+  {
+    return;
+  }
+
+  SendTxBackpackTelemetryViaUDP(dataOutput, packetSize);
+}
+
 void SendCachedMSP()
 {
   if (!cacheFull)