From 403d4dde357d0c1e5aae79dd1bb0ba1eae90f08f Mon Sep 17 00:00:00 2001 From: Nilesh M <285167750+nileshmdev@users.noreply.github.com> Date: Sat, 6 Jun 2026 16:15:16 +0530 Subject: [PATCH] Add BLE CRSF telemetry mode for ESP32C3 TX backpack MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit CRSF telemetry only — MAVLink is not forwarded over BLE. - HM-10 GATT service (0xFFE0/0xFFE1) + Device Information Service - New BACKPACK_TELEM_MODE_BLUETOOTH; CRSF telemetry routed to one transport only - SetSoftMACAddress() early for BLE device name; re-applied before esp_now_init() --- lib/BLE/devBLE.cpp | 217 +++++++++++++++++++++++++++++++++++++++++++ lib/BLE/devBLE.h | 12 +++ src/Tx_main.cpp | 42 +++++++-- targets/txbp_esp.ini | 2 + 4 files changed, 266 insertions(+), 7 deletions(-) create mode 100644 lib/BLE/devBLE.cpp create mode 100644 lib/BLE/devBLE.h diff --git a/lib/BLE/devBLE.cpp b/lib/BLE/devBLE.cpp new file mode 100644 index 00000000..5fd1f037 --- /dev/null +++ b/lib/BLE/devBLE.cpp @@ -0,0 +1,217 @@ +#if defined(PLATFORM_ESP32) && defined(BLE_TELEM_ENABLED) + +#include +#include + +#include "devBLE.h" +#include "common.h" +#include "config.h" +#include "logging.h" +#include "options.h" + +extern TxBackpackConfig config; + +// HM-10 compatible telemetry profile. +static const uint16_t TELEMETRY_SVC_UUID = 0xFFE0; +static const uint16_t TELEMETRY_CRSF_UUID = 0xFFE1; + +// Standard GATT Device Information Service. +static const uint16_t DEVICE_INFO_SVC_UUID = 0x180A; +static const uint16_t MANUFACTURER_NAME_SVC_UUID = 0x2A29; +static const uint16_t MODEL_NUMBER_SVC_UUID = 0x2A24; +static const uint16_t SERIAL_NUMBER_SVC_UUID = 0x2A25; +static const uint16_t SOFTWARE_NUMBER_SVC_UUID = 0x2A28; +static const uint16_t HARDWARE_NUMBER_SVC_UUID = 0x2A27; + +static constexpr uint16_t CRSF_BLE_MAX_PACKET_LEN = 64; // matches MSP_PORT_INBUF_SIZE + +static NimBLEServer *pServer = nullptr; +static NimBLECharacteristic *rcCRSF = nullptr; + +// Single-slot buffer; producer = MSP loop, consumer = timeout(), both on Arduino task. +static uint8_t pendingFrame[CRSF_BLE_MAX_PACKET_LEN] = {0}; +static volatile uint16_t pendingFrameLen = 0; + +static bool justConnected = false; +static uint32_t lastHeartbeat = 0; + +class ServerCallbacks : public NimBLEServerCallbacks +{ + void onConnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo) override + { + justConnected = true; + devicesTriggerEvent(); + DBGLN("BLE client connected"); + } + + void onDisconnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo, int reason) override + { + DBGLN("BLE client disconnected - restarting advertising"); + NimBLEDevice::startAdvertising(); + } + + void onMTUChange(uint16_t MTU, NimBLEConnInfo &connInfo) override + { + DBGLN("BLE MTU updated: %u", MTU); + } +}; +static ServerCallbacks serverCB; + +static void bleStart() +{ + if (pServer != nullptr) + { + return; + } + + // Last 3 UID bytes in device name so multiple backpacks are distinguishable in a scanner. + char devName[24]; + snprintf(devName, sizeof(devName), "ELRS Backpack %02X%02X%02X", + firmwareOptions.uid[3], firmwareOptions.uid[4], firmwareOptions.uid[5]); + + NimBLEDevice::init(devName); + NimBLEDevice::setMTU(512); + + NimBLEServer *server = NimBLEDevice::createServer(); + server->setCallbacks(&serverCB); + + NimBLEService *rcService = server->createService(TELEMETRY_SVC_UUID); + rcCRSF = rcService->createCharacteristic( + TELEMETRY_CRSF_UUID, + NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY, + CRSF_BLE_MAX_PACKET_LEN); + + char serial[18]; + snprintf(serial, sizeof(serial), "%02X:%02X:%02X:%02X:%02X:%02X", + firmwareOptions.uid[0], firmwareOptions.uid[1], firmwareOptions.uid[2], + firmwareOptions.uid[3], firmwareOptions.uid[4], firmwareOptions.uid[5]); + + const char *model = firmwareOptions.product_name[0] != '\0' + ? firmwareOptions.product_name + : "ELRS Backpack"; + + NimBLEService *disService = server->createService(DEVICE_INFO_SVC_UUID); + disService->createCharacteristic(MANUFACTURER_NAME_SVC_UUID, NIMBLE_PROPERTY::READ) + ->setValue("ExpressLRS"); + disService->createCharacteristic(MODEL_NUMBER_SVC_UUID, NIMBLE_PROPERTY::READ) + ->setValue(model); + disService->createCharacteristic(SERIAL_NUMBER_SVC_UUID, NIMBLE_PROPERTY::READ) + ->setValue(serial); + disService->createCharacteristic(SOFTWARE_NUMBER_SVC_UUID, NIMBLE_PROPERTY::READ) + ->setValue("ExpressLRS Backpack"); + disService->createCharacteristic(HARDWARE_NUMBER_SVC_UUID, NIMBLE_PROPERTY::READ) + ->setValue("1.0"); + + server->start(); + + NimBLEAdvertising *pAdvertising = NimBLEDevice::getAdvertising(); + pAdvertising->addServiceUUID(rcService->getUUID()); + pAdvertising->enableScanResponse(true); + pAdvertising->setName(devName); + pAdvertising->start(); + + // Publish pServer last — used as the "BLE ready" gate by SendTxBackpackTelemetryViaBLE(). + pServer = server; + + DBGLN("BLE telemetry started as \"%s\"", devName); +} + +bool SendTxBackpackTelemetryViaBLE(const uint8_t *data, uint16_t size) +{ + if (pServer == nullptr || data == nullptr) + { + return false; + } + if (size == 0 || size > CRSF_BLE_MAX_PACKET_LEN) + { + return false; + } + if (pServer->getConnectedCount() == 0) + { + return false; + } + memcpy(pendingFrame, data, size); + pendingFrameLen = size; + devicesTriggerEvent(); + return true; +} + +static void initialize() +{ + pendingFrameLen = 0; + justConnected = false; +} + +static int start() +{ + // Skip BLE during WiFi update mode — AP + AsyncTCP + BLE on one 2.4GHz radio is unstable. + if (config.GetTelemMode() == BACKPACK_TELEM_MODE_BLUETOOTH && connectionState != wifiUpdate) + { + bleStart(); + } + return DURATION_NEVER; +} + +static int event() +{ + return DURATION_IMMEDIATELY; +} + +static int timeout() +{ + if (pServer == nullptr || rcCRSF == nullptr) + { + return DURATION_NEVER; + } + + if (pServer->getConnectedCount() == 0) + { + return DURATION_NEVER; + } + + // First-connect test frame — lets the client confirm notify is wired up. + if (justConnected) + { + const uint8_t testFrame[] = {0xBE, 0xEF}; + rcCRSF->setValue(testFrame, sizeof(testFrame)); + rcCRSF->notify(); + justConnected = false; + lastHeartbeat = millis(); + return 2000; + } + + if (pendingFrameLen > 0) + { + uint16_t offset = 0; + while (offset < pendingFrameLen) + { + uint16_t chunk = pendingFrameLen - offset; + if (chunk > 20) chunk = 20; + rcCRSF->setValue(pendingFrame + offset, chunk); + rcCRSF->notify(); + offset += chunk; + } + pendingFrameLen = 0; + lastHeartbeat = millis(); + return DURATION_NEVER; + } + + // 2s heartbeat keeps the link alive when no CRSF data is flowing. + if (millis() - lastHeartbeat >= 2000) + { + const uint8_t heartbeat[] = {0xBE, 0xEF}; + rcCRSF->setValue(heartbeat, sizeof(heartbeat)); + rcCRSF->notify(); + lastHeartbeat = millis(); + } + return 2000; +} + +device_t BLE_device = { + .initialize = initialize, + .start = start, + .event = event, + .timeout = timeout, +}; + +#endif diff --git a/lib/BLE/devBLE.h b/lib/BLE/devBLE.h new file mode 100644 index 00000000..5fd70eb6 --- /dev/null +++ b/lib/BLE/devBLE.h @@ -0,0 +1,12 @@ +#pragma once + +#if defined(PLATFORM_ESP32) && defined(BLE_TELEM_ENABLED) + +#include +#include "device.h" + +extern device_t BLE_device; + +bool SendTxBackpackTelemetryViaBLE(const uint8_t *data, uint16_t size); + +#endif diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index 0e38f280..a8246bcf 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -22,6 +22,10 @@ #include "devButton.h" #include "devLED.h" +#if defined(PLATFORM_ESP32) && defined(BLE_TELEM_ENABLED) +#include "devBLE.h" +#endif + #if defined(MAVLINK_ENABLED) #include #endif @@ -48,6 +52,9 @@ device_t *ui_devices[] = { &Button_device, #endif &WIFI_device, +#if defined(PLATFORM_ESP32) && defined(BLE_TELEM_ENABLED) + &BLE_device, +#endif }; /////////// CLASS OBJECTS /////////// @@ -183,6 +190,14 @@ void HandleConfigMsg(mspPacket_t *packet) config.SetStartWiFiOnBoot(true); config.Commit(); break; +#if defined(PLATFORM_ESP32) && defined(BLE_TELEM_ENABLED) + case BACKPACK_TELEM_MODE_BLUETOOTH: + config.SetTelemMode(BACKPACK_TELEM_MODE_BLUETOOTH); + config.SetWiFiService(WIFI_SERVICE_UPDATE); + config.SetStartWiFiOnBoot(false); + config.Commit(); + break; +#endif } rebootTime = millis(); break; @@ -225,13 +240,23 @@ 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) + // Route CRSF telemetry to one transport only — avoids radio contention with BLE. + switch (config.GetTelemMode()) { - sendMSPViaEspnow(packet); + case BACKPACK_TELEM_MODE_ESPNOW: + sendMSPViaEspnow(packet); + break; + case BACKPACK_TELEM_MODE_WIFI: + sendMSPViaWiFiUDP(packet); + break; +#if defined(PLATFORM_ESP32) && defined(BLE_TELEM_ENABLED) + case BACKPACK_TELEM_MODE_BLUETOOTH: + SendTxBackpackTelemetryViaBLE(packet->payload, packet->payloadSize); + break; +#endif + case BACKPACK_TELEM_MODE_OFF: + default: + break; } break; @@ -392,13 +417,15 @@ void setup() config.SetStorageProvider(&eeprom); config.Load(); + SetSoftMACAddress(); + devicesInit(ui_devices, ARRAY_SIZE(ui_devices)); #ifdef DEBUG_ELRS_WIFI config.SetStartWiFiOnBoot(true); #endif - + if (config.GetStartWiFiOnBoot()) { wifiService = config.GetWiFiService(); @@ -412,6 +439,7 @@ void setup() } else { + // Re-apply: devicesInit()→wifiOff() turned WiFi off after the early call. SetSoftMACAddress(); if (esp_now_init() != 0) diff --git a/targets/txbp_esp.ini b/targets/txbp_esp.ini index 4e5b2c33..9e9399e0 100644 --- a/targets/txbp_esp.ini +++ b/targets/txbp_esp.ini @@ -34,10 +34,12 @@ extends = env_common_esp32c3, tx_backpack_common lib_deps = ${tx_backpack_common.lib_deps} ${common_env_data.mavlink_lib_dep} + h2zero/NimBLE-Arduino @ ^2.5.0 build_flags = ${env_common_esp32c3.build_flags} ${tx_backpack_common.build_flags} -D MAVLINK_ENABLED=1 + -D BLE_TELEM_ENABLED=1 -D PIN_BUTTON=9 -D PIN_LED=8 upload_resetmethod = nodemcu