diff --git a/lib/WIFI/devWIFI.cpp b/lib/WIFI/devWIFI.cpp index 88a1f1fa..597526f6 100644 --- a/lib/WIFI/devWIFI.cpp +++ b/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) { diff --git a/lib/WIFI/devWIFI.h b/lib/WIFI/devWIFI.h index 3b96cc69..57bfc922 100644 --- a/lib/WIFI/devWIFI.h +++ b/lib/WIFI/devWIFI.h @@ -7,4 +7,8 @@ extern device_t WIFI_device; #define HAS_WIFI extern const char *VERSION; -#endif \ No newline at end of file + +#if defined(TARGET_TX_BACKPACK) +bool SendTxBackpackTelemetryViaUDP(const uint8_t *data, uint16_t size); +#endif +#endif diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index 82d06baf..6e7d31db 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -64,6 +64,7 @@ MAVLink mavlink; /////////// FUNCTION DEFS /////////// void sendMSPViaEspnow(mspPacket_t *packet); +void sendMSPViaWiFiUDP(mspPacket_t *packet); ///////////////////////////////////// @@ -224,7 +225,12 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) case MSP_ELRS_BACKPACK_CRSF_TLM: DBGLN("Processing MSP_ELRS_BACKPACK_CRSF_TLM..."); - if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF) + if (config.GetTelemMode() == BACKPACK_TELEM_MODE_WIFI) + { + sendMSPViaWiFiUDP(packet); + sendMSPViaEspnow(packet); + } + else if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF) { sendMSPViaEspnow(packet); } @@ -288,6 +294,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)