From 41d81c0f26c75c70c00a64af8c6b7995f43266e2 Mon Sep 17 00:00:00 2001 From: xNuclearSquirrel Date: Wed, 18 Feb 2026 18:57:28 +0100 Subject: [PATCH 1/3] added CRSF telemetry via UDP broadcast --- lib/WIFI/devWIFI.cpp | 42 ++++++++++++++++++++++++++++++++++++++++++ lib/WIFI/devWIFI.h | 6 +++++- src/Tx_main.cpp | 21 ++++++++++++++++++++- 3 files changed, 67 insertions(+), 2 deletions(-) diff --git a/lib/WIFI/devWIFI.cpp b/lib/WIFI/devWIFI.cpp index 88a1f1fa..2853b99f 100644 --- a/lib/WIFI/devWIFI.cpp +++ b/lib/WIFI/devWIFI.cpp @@ -119,6 +119,48 @@ 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..5cb17c3c 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -64,6 +64,7 @@ MAVLink mavlink; /////////// FUNCTION DEFS /////////// void sendMSPViaEspnow(mspPacket_t *packet); +bool sendMSPViaWiFiUDP(mspPacket_t *packet); ///////////////////////////////////// @@ -224,7 +225,11 @@ 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); + } + else if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF) { sendMSPViaEspnow(packet); } @@ -288,6 +293,20 @@ void sendMSPViaEspnow(mspPacket_t *packet) blinkLED(); } +bool sendMSPViaWiFiUDP(mspPacket_t *packet) +{ + uint8_t packetSize = msp.getTotalPacketSize(packet); + uint8_t dataOutput[packetSize]; + + uint8_t result = msp.convertToByteArray(packet, dataOutput); + if (!result) + { + return false; + } + + return SendTxBackpackTelemetryViaUDP(dataOutput, result); +} + void SendCachedMSP() { if (!cacheFull) From fc02a259069ed2aed2f6f21e54269ae2cf8bad4b Mon Sep 17 00:00:00 2001 From: xNuclearSquirrel Date: Wed, 18 Feb 2026 22:15:33 +0100 Subject: [PATCH 2/3] enabled espnow also in wifi mode --- src/Tx_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index 5cb17c3c..0a11f760 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -228,6 +228,7 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) if (config.GetTelemMode() == BACKPACK_TELEM_MODE_WIFI) { sendMSPViaWiFiUDP(packet); + sendMSPViaEspnow(packet); } else if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF) { From 973628d831242eff06297afdb4bfa82fb7b925b8 Mon Sep 17 00:00:00 2001 From: xNuclearSquirrel Date: Thu, 19 Feb 2026 00:06:12 +0100 Subject: [PATCH 3/3] style changes --- lib/WIFI/devWIFI.cpp | 26 ++++++++++++++++++-------- src/Tx_main.cpp | 8 ++++---- 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/lib/WIFI/devWIFI.cpp b/lib/WIFI/devWIFI.cpp index 2853b99f..597526f6 100644 --- a/lib/WIFI/devWIFI.cpp +++ b/lib/WIFI/devWIFI.cpp @@ -122,36 +122,46 @@ bool gcsIPSet = false; #if defined(TARGET_TX_BACKPACK) bool SendTxBackpackTelemetryViaUDP(const uint8_t *data, uint16_t size) { - if (!data || size == 0) { + if (!data || size == 0) + { return false; } #if !defined(MAVLINK_ENABLED) return false; #else - if (!servicesStarted || !wifiStarted) { + if (!servicesStarted || !wifiStarted) + { return false; } - if (wifiService != WIFI_SERVICE_MAVLINK_TX) { + if (wifiService != WIFI_SERVICE_MAVLINK_TX) + { return false; } IPAddress remote; WiFiMode_t mode = WiFi.getMode(); - if (mode == WIFI_AP || mode == WIFI_AP_STA) { + if (mode == WIFI_AP || mode == WIFI_AP_STA) + { remote = apBroadcast; - } else if (mode == WIFI_STA) { + } + else if (mode == WIFI_STA) + { remote = WiFi.broadcastIP(); - } else { + } + else + { return false; } - if (!mavlinkUDP.beginPacket(remote, config.GetMavlinkSendPort())) { + if (!mavlinkUDP.beginPacket(remote, config.GetMavlinkSendPort())) + { return false; } size_t sent = mavlinkUDP.write(data, size); - if (sent != size) { + if (sent != size) + { mavlinkUDP.endPacket(); return false; } diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index 0a11f760..6e7d31db 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -64,7 +64,7 @@ MAVLink mavlink; /////////// FUNCTION DEFS /////////// void sendMSPViaEspnow(mspPacket_t *packet); -bool sendMSPViaWiFiUDP(mspPacket_t *packet); +void sendMSPViaWiFiUDP(mspPacket_t *packet); ///////////////////////////////////// @@ -294,7 +294,7 @@ void sendMSPViaEspnow(mspPacket_t *packet) blinkLED(); } -bool sendMSPViaWiFiUDP(mspPacket_t *packet) +void sendMSPViaWiFiUDP(mspPacket_t *packet) { uint8_t packetSize = msp.getTotalPacketSize(packet); uint8_t dataOutput[packetSize]; @@ -302,10 +302,10 @@ bool sendMSPViaWiFiUDP(mspPacket_t *packet) uint8_t result = msp.convertToByteArray(packet, dataOutput); if (!result) { - return false; + return; } - return SendTxBackpackTelemetryViaUDP(dataOutput, result); + SendTxBackpackTelemetryViaUDP(dataOutput, packetSize); } void SendCachedMSP()