diff --git a/html/scan.js b/html/scan.js
index 4ee678be..6f80ae54 100644
--- a/html/scan.js
+++ b/html/scan.js
@@ -7,6 +7,7 @@ function _(el) {
function init() {
initAat();
initMavLink();
+ initBleTrainer();
// sends XMLHttpRequest, so do it last
initOptions();
@@ -47,6 +48,55 @@ function initMavLink() {
xmlhttp.open('GET', '/mavlink', true);
xmlhttp.send();
}
+
+function initBleTrainer() {
+ const form = _('bt_form');
+ if (!form)
+ return;
+ form.addEventListener('submit', (e) => { e.preventDefault(); btSubmit(false); });
+ _('bt_forget').addEventListener('click', (e) => { e.preventDefault(); btSubmit(true); });
+ _('bt_source').addEventListener('change', () => btToggleBleSettings(_('bt_source').value));
+}
+
+// Posts the FrSky BLE Trainer form (or a Forget request) and updates the page
+// in place, so no manual reload is needed.
+function btSubmit(clearPeer) {
+ const body = clearPeer ? new URLSearchParams({ forget: 1 })
+ : new URLSearchParams(new FormData(_('bt_form')));
+ const xmlhttp = new XMLHttpRequest();
+ xmlhttp.onreadystatechange = function () {
+ if (this.readyState != 4)
+ return;
+ if (this.status == 200) {
+ if (clearPeer) {
+ _('bt_status').textContent = 'Not paired';
+ _('bt_mac').textContent = '-';
+ }
+ cuteAlert({
+ type: 'success',
+ title: 'Bluetooth Trainer',
+ message: 'Bluetooth trainer settings saved'
+ });
+ } else {
+ cuteAlert({
+ type: 'error',
+ title: 'Bluetooth Trainer',
+ message: 'An error occurred saving the Bluetooth trainer settings'
+ });
+ }
+ };
+ xmlhttp.open('POST', '/settrainer', true);
+ xmlhttp.setRequestHeader('Content-type', 'application/x-www-form-urlencoded');
+ xmlhttp.send(body);
+}
+
+// Bluetooth-only fields are hidden when the native source is selected.
+function btToggleBleSettings(source) {
+ const el = _('bt_ble_settings');
+ if (el)
+ el.style.display = (source === 'native') ? 'none' : 'block';
+}
+
function initOptions() {
const xmlhttp = new XMLHttpRequest();
xmlhttp.onreadystatechange = function() {
@@ -136,6 +186,15 @@ function updateConfig(data) {
if (config['head-tracking']) {
if (_('httab')) _('httab').style.display = 'table-cell';
}
+ if (config['trainer']) {
+ if (_('bttab')) _('bttab').style.display = 'table-cell';
+ const bt = config.trainer;
+ _('bt_status').textContent = bt.paired ? 'Paired' : 'Not paired';
+ _('bt_mac').textContent = bt.paired ? bt.mac : '-';
+ _('bt_interval').value = bt.interval;
+ _('bt_source').value = bt.source;
+ btToggleBleSettings(bt.source);
+ }
if((!data.stm32 || data.stm32==="no") && _('tx_tab')) {
mui.tabs.activate('pane-justified-2');
_('tx_tab').style.display = 'none';
diff --git a/html/txbp_index.html b/html/txbp_index.html
index 08c78af2..44190928 100644
--- a/html/txbp_index.html
+++ b/html/txbp_index.html
@@ -26,6 +26,7 @@
ExpressLRS
Backpack
Network
MavLink
+ FrSky BLE Trainer
@@ -146,6 +147,48 @@
MavLink Statistics
+
+
+
FrSky BLE Trainer
+
The backpack forwards trainer channels to EdgeTX (as trainer input) from one of
+ two sources: the native system (ESP-NOW, e.g. a VRX or goggles with IMU) or a
+ Bluetooth device advertising the FrSky trainer service (e.g. an Arduino-based
+ HeadTracker). Both respect the radio's HT Enable; use the selector to choose which one
+ is used.
+
+
diff --git a/include/BleTrainer.h b/include/BleTrainer.h
new file mode 100644
index 00000000..f5384aa1
--- /dev/null
+++ b/include/BleTrainer.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "device.h"
+
+// Bluetooth trainer bridge: the backpack acts as a BLE central and connects to
+// a device that advertises the FrSky trainer service (0xFFF0/0xFFF6), such as a
+// HeadTracker, parses the FrSky trainer frame and emits
+// MSP_ELRS_BACKPACK_SET_PTR over Serial towards the ELRS module.
+//
+// Compiled only on BLE-capable targets (ESP32-C3/S3/ESP32) with HAS_BLE_TRAINER.
+
+#if defined(HAS_BLE_TRAINER)
+
+extern device_t BleTrainer_device;
+
+// True while a trainer is connected over BLE.
+bool bleTrainerConnected();
+
+// Forget the paired peer and go back to pairing mode (button long press).
+void bleTrainerForgetPeer();
+
+// Retained-RAM diagnostic: number of forwarded trainer packets, read from /config
+// (the Nomad backpack has no USB console).
+uint32_t bleTrainerSetPtrCount();
+
+#endif
diff --git a/lib/BUTTON/devButton.cpp b/lib/BUTTON/devButton.cpp
index 23ba847c..f7396ffc 100644
--- a/lib/BUTTON/devButton.cpp
+++ b/lib/BUTTON/devButton.cpp
@@ -6,6 +6,7 @@
#if defined(PIN_BUTTON)
#include "logging.h"
#include "button.h"
+#include "BleTrainer.h"
static Button button;
@@ -24,9 +25,21 @@ static void shortPress()
}
}
+#if defined(HAS_BLE_TRAINER)
+// Long press: forget the paired Bluetooth trainer so it can be re-paired.
+static void longPress()
+{
+ DBGLN("Button long: forget Bluetooth trainer peer");
+ bleTrainerForgetPeer();
+}
+#endif
+
static void initialize()
{
button.OnShortPress = shortPress;
+#if defined(HAS_BLE_TRAINER)
+ button.OnLongPress = longPress;
+#endif
}
static int start()
diff --git a/lib/WIFI/devWIFI.cpp b/lib/WIFI/devWIFI.cpp
index 26b4013f..03ce19bd 100644
--- a/lib/WIFI/devWIFI.cpp
+++ b/lib/WIFI/devWIFI.cpp
@@ -52,6 +52,11 @@ extern wifi_service_t wifiService;
#if defined(MAVLINK_ENABLED)
extern MAVLink mavlink;
#endif
+#if defined(HAS_BLE_TRAINER)
+#include
+#include
+#include "BleTrainer.h"
+#endif
#elif defined(TARGET_TIMER_BACKPACK)
extern TimerBackpackConfig config;
#else
@@ -103,6 +108,65 @@ extern bool headTrackingEnabled;
#endif
static bool servicesStarted = false;
+#if defined(TARGET_TX_BACKPACK)
+static void setMavlinkAccessPointSSID()
+{
+#if defined(PLATFORM_ESP32)
+ static const uint8_t unboundAddress[6] = {0, 0, 0, 0, 0, 0};
+ if (memcmp(firmwareOptions.uid, unboundAddress, sizeof(unboundAddress)) == 0)
+ {
+ const uint32_t suffix = (uint32_t)(ESP.getEfuseMac() & 0xFFFFFF);
+ snprintf(wifi_ap_ssid, sizeof(wifi_ap_ssid), "ExpressLRS TX Backpack %06lX",
+ (unsigned long)suffix);
+ return;
+ }
+#endif
+ snprintf(wifi_ap_ssid, sizeof(wifi_ap_ssid), "ExpressLRS TX Backpack %02X%02X%02X",
+ firmwareOptions.uid[3], firmwareOptions.uid[4], firmwareOptions.uid[5]);
+}
+#endif
+
+#if defined(HAS_BLE_TRAINER)
+// Boot forensics kept in retained RAM: the Nomad has no USB console, and the
+// MAVLink WiFi crash loop can only be diagnosed by reading, on the next boot,
+// what ended the previous one. Survives software/watchdog/brownout resets.
+#define TRAINER_BOOT_FX_MAGIC 0x48544246UL
+struct TrainerBootForensics
+{
+ uint32_t magic;
+ uint32_t boots;
+};
+RTC_NOINIT_ATTR static TrainerBootForensics s_bootFx;
+static uint32_t s_bootCount = 0;
+static int32_t s_thisReset = ESP_RST_UNKNOWN;
+
+static const char *resetReasonName(int32_t reason)
+{
+ switch (reason)
+ {
+ case ESP_RST_POWERON: return "power-on";
+ case ESP_RST_EXT: return "external-pin";
+ case ESP_RST_SW: return "software";
+ case ESP_RST_PANIC: return "panic";
+ case ESP_RST_INT_WDT: return "interrupt-watchdog";
+ case ESP_RST_TASK_WDT: return "task-watchdog";
+ case ESP_RST_WDT: return "other-watchdog";
+ case ESP_RST_DEEPSLEEP: return "deep-sleep";
+ case ESP_RST_BROWNOUT: return "brownout";
+ case ESP_RST_SDIO: return "sdio";
+ default: return "unknown";
+ }
+}
+
+static void captureBootForensics()
+{
+ s_thisReset = (int32_t)esp_reset_reason();
+ s_bootCount = (s_bootFx.magic == TRAINER_BOOT_FX_MAGIC) ? s_bootFx.boots + 1 : 1;
+ s_bootFx.magic = TRAINER_BOOT_FX_MAGIC;
+ s_bootFx.boots = s_bootCount;
+}
+#endif
+
static bool target_seen = false;
static uint8_t target_pos = 0;
static String target_found;
@@ -314,6 +378,23 @@ static void GetConfiguration(AsyncWebServerRequest *request)
#if defined(HAS_HEADTRACKING) || defined(SUPPORT_HEADTRACKING)
json["config"]["head-tracking"] = true;
#endif
+#if defined(HAS_BLE_TRAINER)
+ json["config"]["trainer"]["paired"] = config.IsTrainerPaired();
+ json["config"]["trainer"]["interval"] = config.GetTrainerIntervalMs();
+ const uint8_t *trainerMac = config.GetTrainerPeerMac();
+ char trainerMacStr[18];
+ snprintf(trainerMacStr, sizeof(trainerMacStr), "%02X:%02X:%02X:%02X:%02X:%02X",
+ trainerMac[0], trainerMac[1], trainerMac[2], trainerMac[3], trainerMac[4], trainerMac[5]);
+ json["config"]["trainer"]["mac"] = trainerMacStr;
+ const char *trainerSource = "auto";
+ if (config.GetTrainerSource() == TRAINER_SOURCE_ESPNOW) trainerSource = "native";
+ else if (config.GetTrainerSource() == TRAINER_SOURCE_BLE) trainerSource = "ble";
+ json["config"]["trainer"]["source"] = trainerSource;
+ json["config"]["trainer"]["boots"] = s_bootCount;
+ json["config"]["trainer"]["lastreset"] = resetReasonName(s_thisReset);
+ json["config"]["trainer"]["linked"] = bleTrainerConnected();
+ json["config"]["trainer"]["setptr"] = bleTrainerSetPtrCount();
+#endif
#if defined(AAT_BACKPACK)
WebAatAppendConfig(json);
#endif
@@ -621,6 +702,34 @@ static void WebUpdateSetMavLink(AsyncWebServerRequest *request)
}
#endif
+#if defined(HAS_BLE_TRAINER)
+static void WebUpdateSetTrainer(AsyncWebServerRequest *request)
+{
+ if (request->hasArg("interval"))
+ {
+ int interval = request->arg("interval").toInt();
+ config.SetTrainerIntervalMs((uint16_t)constrain(interval, 7, 1000));
+ }
+ if (request->hasArg("source"))
+ {
+ String source = request->arg("source");
+ if (source == "native" || source == "espnow")
+ config.SetTrainerSource(TRAINER_SOURCE_ESPNOW);
+ else if (source == "ble")
+ config.SetTrainerSource(TRAINER_SOURCE_BLE);
+ else
+ config.SetTrainerSource(TRAINER_SOURCE_AUTO);
+ }
+ if (request->hasArg("forget") && request->arg("forget") != "0")
+ {
+ config.ClearTrainerPeerMac();
+ }
+ config.Commit();
+ DBGLN("Bluetooth trainer configuration updated");
+ request->send(200, "text/plain", "Bluetooth trainer settings saved");
+}
+#endif
+
static void wifiOff()
{
wifiStarted = false;
@@ -748,6 +857,9 @@ static void startServices()
#if defined(MAVLINK_ENABLED)
server.on("/setmavlink", WebUpdateSetMavLink);
#endif
+ #if defined(HAS_BLE_TRAINER)
+ server.on("/settrainer", HTTP_POST, WebUpdateSetTrainer);
+ #endif
server.on("/forget", WebUpdateForget);
server.on("/connect", WebUpdateConnect);
server.on("/access", WebUpdateAccessPoint);
@@ -800,6 +912,15 @@ static void startServices()
static void HandleWebUpdate()
{
unsigned long now = millis();
+#if defined(HAS_BLE_TRAINER)
+ static unsigned long lastStatusLog = 0;
+ if (now - lastStatusLog > 3000)
+ {
+ lastStatusLog = now;
+ DBGLN("WIFI status mode=%d started=%d services=%d ap=%s", (int)wifiMode, wifiStarted,
+ servicesStarted, WiFi.softAPIP().toString().c_str());
+ }
+#endif
wl_status_t status = WiFi.status();
if (status != laststatus && wifiMode == WIFI_STA) {
DBGLN("WiFi status %d", status);
@@ -843,12 +964,7 @@ static void HandleWebUpdate()
}
else if (wifiService == WIFI_SERVICE_MAVLINK_TX)
{
- // Generate a unique SSID using config.address as hex
- sprintf(wifi_ap_ssid, "ExpressLRS TX Backpack %02X%02X%02X",
- firmwareOptions.uid[3],
- firmwareOptions.uid[4],
- firmwareOptions.uid[5]
- );
+ setMavlinkAccessPointSSID();
}
#endif
WiFi.softAP(wifi_ap_ssid, wifi_ap_password);
@@ -945,11 +1061,10 @@ static void HandleWebUpdate()
#endif
// When in STA mode, a small delay reduces power use from 90mA to 30mA when idle
// In AP mode, it doesn't seem to make a measurable difference, but does not hurt
-#if defined(MAVLINK_ENABLED)
- if (!updater.isRunning() && wifiService != WIFI_SERVICE_MAVLINK_TX)
-#else
+ // The ESP32-C3 is single-core: always yield to the idle task here. Skipping
+ // the delay in MAVLINK_TX lets this tight poll loop starve the idle task
+ // once BLE adds its radio tasks, which resets the whole chip.
if (!updater.isRunning())
-#endif
delay(1);
if (do_flash) {
do_flash = false;
@@ -995,6 +1110,9 @@ static void HandleWebUpdate()
static int start()
{
+#if defined(HAS_BLE_TRAINER)
+ captureBootForensics();
+#endif
return DURATION_NEVER;
}
diff --git a/lib/config/config.cpp b/lib/config/config.cpp
index 50e1316f..d7efefab 100644
--- a/lib/config/config.cpp
+++ b/lib/config/config.cpp
@@ -15,6 +15,18 @@ TxBackpackConfig::Load()
// If not, revert to defaults for this version
DBGLN("EEPROM version mismatch! Resetting to defaults...");
SetDefaults();
+ return;
+ }
+
+ // The stored connection interval can predate the throttling fix, in which case
+ // it is the old default rather than a deliberate choice. Migrate it so the fix
+ // reaches units that were paired before it landed.
+ if (m_config.trainerIntervalMs == TRAINER_LEGACY_INTERVAL_MS)
+ {
+ DBGLN("Migrating legacy trainer interval to default");
+ m_config.trainerIntervalMs = TRAINER_DEFAULT_INTERVAL_MS;
+ m_modified = true;
+ Commit();
}
}
@@ -54,6 +66,13 @@ TxBackpackConfig::SetDefaults()
m_config.wifiService = WIFI_SERVICE_UPDATE;
m_config.mavlinkListenPort = 14555; // Default MavLink listen port
m_config.mavlinkSendPort = 14550; // Default MavLink send port
+ memset(m_config.trainerPeerMac, 0, 6);
+ m_config.trainerPeerType = 0;
+ // The trainer only has one notification in flight at a time, so its
+ // update rate is capped at one frame per connection event. Keep this small
+ // enough not to throttle the stream below the device's own 80 Hz rate.
+ m_config.trainerIntervalMs = TRAINER_DEFAULT_INTERVAL_MS;
+ m_config.trainerSource = TRAINER_SOURCE_AUTO;
m_modified = true;
Commit();
}
@@ -112,6 +131,53 @@ TxBackpackConfig::SetMavlinkSendPort(uint16_t port)
m_config.mavlinkSendPort = port;
m_modified = true;
}
+
+bool
+TxBackpackConfig::IsTrainerPaired() const
+{
+ for (int i = 0; i < 6; i++)
+ {
+ if (m_config.trainerPeerMac[i] != 0)
+ return true;
+ }
+ return false;
+}
+
+void
+TxBackpackConfig::SetTrainerPeerMac(const uint8_t mac[6])
+{
+ memcpy(m_config.trainerPeerMac, mac, 6);
+ m_modified = true;
+}
+
+void
+TxBackpackConfig::SetTrainerPeerType(uint8_t type)
+{
+ m_config.trainerPeerType = type;
+ m_modified = true;
+}
+
+void
+TxBackpackConfig::ClearTrainerPeerMac()
+{
+ memset(m_config.trainerPeerMac, 0, 6);
+ m_config.trainerPeerType = 0;
+ m_modified = true;
+}
+
+void
+TxBackpackConfig::SetTrainerIntervalMs(uint16_t ms)
+{
+ m_config.trainerIntervalMs = ms;
+ m_modified = true;
+}
+
+void
+TxBackpackConfig::SetTrainerSource(trainer_source_t source)
+{
+ m_config.trainerSource = source;
+ m_modified = true;
+}
#endif
/////////////////////////////////////////////////////
diff --git a/lib/config/config.h b/lib/config/config.h
index 11692e9a..ed213e98 100644
--- a/lib/config/config.h
+++ b/lib/config/config.h
@@ -7,10 +7,16 @@
#define VRX_BACKPACK_CONFIG_MAGIC (0b10U << 30)
#define TIMER_BACKPACK_CONFIG_MAGIC (0b11U << 30)
-#define TX_BACKPACK_CONFIG_VERSION 4
+#define TX_BACKPACK_CONFIG_VERSION 6
#define VRX_BACKPACK_CONFIG_VERSION 5
#define TIMER_BACKPACK_CONFIG_VERSION 3
+// The Bluetooth trainer stream used to default to a 100 ms BLE connection interval,
+// which capped the bridge at 10 Hz against the device's own 80 Hz rate. Treat that
+// legacy value as "never explicitly configured" so already-paired units pick up the fix.
+#define TRAINER_LEGACY_INTERVAL_MS 100
+#define TRAINER_DEFAULT_INTERVAL_MS 8
+
typedef enum {
WIFI_SERVICE_UPDATE,
@@ -24,6 +30,13 @@ typedef enum {
BACKPACK_TELEM_MODE_BLUETOOTH,
} telem_mode_t;
+// Trainer source selection
+typedef enum {
+ TRAINER_SOURCE_AUTO, // BLE if connected, otherwise ESP-NOW (VRX)
+ TRAINER_SOURCE_ESPNOW, // ESP-NOW (VRX) only
+ TRAINER_SOURCE_BLE, // BLE only
+} trainer_source_t;
+
#if defined(TARGET_TX_BACKPACK)
typedef struct {
@@ -36,6 +49,11 @@ typedef struct {
telem_mode_t telemMode;
uint16_t mavlinkListenPort;
uint16_t mavlinkSendPort;
+ // Bluetooth trainer (central). All-zero MAC = not paired.
+ uint8_t trainerPeerMac[6];
+ uint8_t trainerPeerType; // NimBLE address type
+ uint16_t trainerIntervalMs; // requested BLE connection interval (ms)
+ trainer_source_t trainerSource; // trainer source selection
} tx_backpack_config_t;
class TxBackpackConfig
@@ -54,6 +72,11 @@ class TxBackpackConfig
telem_mode_t GetTelemMode() { return m_config.telemMode; }
uint16_t GetMavlinkListenPort() const { return m_config.mavlinkListenPort; }
uint16_t GetMavlinkSendPort() const { return m_config.mavlinkSendPort; }
+ uint8_t *GetTrainerPeerMac() { return m_config.trainerPeerMac; }
+ uint8_t GetTrainerPeerType() { return m_config.trainerPeerType; }
+ uint16_t GetTrainerIntervalMs() { return m_config.trainerIntervalMs; }
+ trainer_source_t GetTrainerSource() { return m_config.trainerSource; }
+ bool IsTrainerPaired() const;
// Setters
void SetStorageProvider(ELRS_EEPROM *eeprom);
@@ -66,6 +89,11 @@ class TxBackpackConfig
void SetTelemMode(telem_mode_t mode);
void SetMavlinkListenPort(uint16_t port);
void SetMavlinkSendPort(uint16_t port);
+ void SetTrainerPeerMac(const uint8_t mac[6]);
+ void SetTrainerPeerType(uint8_t type);
+ void ClearTrainerPeerMac();
+ void SetTrainerIntervalMs(uint16_t ms);
+ void SetTrainerSource(trainer_source_t source);
private:
tx_backpack_config_t m_config;
diff --git a/src/BleTrainer.cpp b/src/BleTrainer.cpp
new file mode 100644
index 00000000..153ecb44
--- /dev/null
+++ b/src/BleTrainer.cpp
@@ -0,0 +1,404 @@
+#include
+
+#if defined(HAS_BLE_TRAINER)
+
+#include
+#include
+#include
+
+#include "BleTrainer.h"
+#include "common.h"
+#include "config.h"
+#include "device.h"
+#include "msp.h"
+#include "msptypes.h"
+
+extern MSP msp;
+extern bool headTrackingEnabled;
+extern wifi_service_t wifiService;
+
+// ------------------------------------------------------------------
+// FrSky Bluetooth trainer framing parser (same as BTWifiModule)
+// 0x7E delimiter, 0x7D byte-stuff (XOR 0x20), frame 0x80 = 8 channels
+// The trainer frame is 0x80 + 12 packed channel bytes + XOR checksum. The
+// CRC and end delimiter are sent verbatim (not byte-stuffed); decode one
+// complete report per BLE notification and ignore the remaining padding.
+// ------------------------------------------------------------------
+#define FRSKY_CHANNELS 8
+#define FRSKY_TRAINER_PACKET_SIZE 14
+static constexpr uint16_t FRSKY_PWM_MIN_US = 988;
+static constexpr uint16_t FRSKY_PWM_MAX_US = 2012;
+static constexpr uint16_t FRSKY_CRSF_MIN_VALUE = 172;
+static constexpr uint16_t FRSKY_CRSF_MAX_VALUE = 1811;
+
+static volatile uint16_t frskyChannels[FRSKY_CHANNELS];
+static volatile bool s_gotFrame = false;
+
+static void frskyHandleFrame(const uint8_t *frame, uint8_t len)
+{
+ if (len < 2)
+ return;
+
+ uint8_t crc = 0;
+ for (uint8_t i = 0; i < len - 1; i++)
+ crc ^= frame[i];
+ if (crc != frame[len - 1])
+ return;
+
+ if (len == FRSKY_TRAINER_PACKET_SIZE && frame[0] == 0x80)
+ {
+ for (uint8_t ch = 0, i = 1; ch < FRSKY_CHANNELS; ch += 2, i += 3)
+ {
+ frskyChannels[ch] = frame[i] + ((frame[i + 1] & 0xF0) << 4);
+ frskyChannels[ch + 1] = ((frame[i + 1] & 0x0F) << 4) +
+ ((frame[i + 2] & 0xF0) >> 4) +
+ ((frame[i + 2] & 0x0F) << 8);
+ }
+
+ s_gotFrame = true;
+ }
+}
+
+// Each BLE notification contains one complete FrSky trainer report followed by
+// padding. Decode within that notification and ignore the padding rather than
+// carrying parser state across notifications.
+static void frskyProcessNotification(const uint8_t *data, size_t len)
+{
+ if (len < 2 || data[0] != 0x7E)
+ return;
+
+ uint8_t frame[FRSKY_TRAINER_PACKET_SIZE];
+ size_t pos = 1;
+ // The type and 12 packed channel bytes are byte-stuffed; the CRC is not.
+ for (uint8_t i = 0; i < FRSKY_TRAINER_PACKET_SIZE - 1; i++)
+ {
+ if (pos >= len)
+ return;
+
+ uint8_t b = data[pos++];
+ if (b == 0x7D)
+ {
+ if (pos >= len)
+ return;
+ b = data[pos++] ^ 0x20;
+ }
+ else if (b == 0x7E)
+ return;
+ frame[i] = b;
+ }
+
+ if (pos >= len)
+ return;
+ frame[FRSKY_TRAINER_PACKET_SIZE - 1] = data[pos++];
+ if (pos >= len || data[pos] != 0x7E)
+ return;
+
+ frskyHandleFrame(frame, FRSKY_TRAINER_PACKET_SIZE);
+}
+
+// ------------------------------------------------------------------
+// BLE central (NimBLE)
+// ------------------------------------------------------------------
+// The trainer source never queues more than one notification: it gives its send
+// semaphore back from the notification-complete callback, so its update rate is
+// capped at one frame per connection event (its own thread runs at 80 Hz). It
+// also asks for its own preferred parameters from its connection callback and
+// accepts whatever the central asks for, so the interval requested here has to
+// be small enough not to throttle the stream.
+#define TRAINER_MIN_INTERVAL_MS 7
+#define TRAINER_MAX_INTERVAL_MS 1000
+// Long enough to ride out a transient RF blockage (a few hundred ms of missed
+// connection events next to a 1 W RF module) instead of dropping the link, which
+// would cost a reconnect and make the trainer report it as lost.
+#define TRAINER_SUPERVISION_TIMEOUT 400
+// Let the trainer's own parameter request land before ours.
+#define TRAINER_CONN_PARAM_DELAY_MS 1200
+
+static NimBLEScan *s_scan = nullptr;
+static NimBLEClient *s_client = nullptr;
+static volatile bool s_connected = false;
+static volatile bool s_found = false;
+static volatile bool s_forget = false;
+static int s_failCount = 0;
+static NimBLEAddress s_foundAddr;
+static int s_bestRssi = -127;
+
+// Retained-RAM diagnostic: the Nomad has no console, so the count of forwarded
+// trainer packets survives reboots and is read from /config.
+#define TRAINER_FX_MAGIC 0x48544658UL
+RTC_NOINIT_ATTR static uint32_t s_setPtrMagic;
+RTC_NOINIT_ATTR static uint32_t s_setPtrCount;
+
+static void setPtrTick()
+{
+ if (s_setPtrMagic != TRAINER_FX_MAGIC)
+ {
+ s_setPtrMagic = TRAINER_FX_MAGIC;
+ s_setPtrCount = 0;
+ }
+ s_setPtrCount++;
+}
+
+static void getRequestedConnParams(uint16_t &minU, uint16_t &maxU)
+{
+ uint16_t iv = config.GetTrainerIntervalMs();
+ if (iv < TRAINER_MIN_INTERVAL_MS) iv = TRAINER_MIN_INTERVAL_MS;
+ if (iv > TRAINER_MAX_INTERVAL_MS) iv = TRAINER_MAX_INTERVAL_MS;
+ // min/max interval are in 1.25 ms units, supervision timeout in 10 ms units.
+ minU = (uint16_t)(iv * 4 / 5);
+ if (minU < 6) minU = 6;
+ maxU = (uint16_t)((iv + 5) * 4 / 5);
+ if (maxU < minU) maxU = minU;
+}
+
+static void setInitialConnParams()
+{
+ uint16_t minU, maxU;
+ getRequestedConnParams(minU, maxU);
+ // NimBLE otherwise starts at its defaults (20-50 ms, 2560 ms timeout).
+ // Set these before connect so the first connection event uses the trainer rate.
+ s_client->setConnectionParams(minU, maxU, 0, TRAINER_SUPERVISION_TIMEOUT);
+}
+
+static void applyConnParams()
+{
+ uint16_t minU, maxU;
+ getRequestedConnParams(minU, maxU);
+ s_client->updateConnParams(minU, maxU, 0, TRAINER_SUPERVISION_TIMEOUT);
+}
+
+static void onNotify(NimBLERemoteCharacteristic *, uint8_t *data, size_t len, bool)
+{
+ frskyProcessNotification(data, len);
+}
+
+class TrainerScanCb : public NimBLEAdvertisedDeviceCallbacks
+{
+ void onResult(NimBLEAdvertisedDevice *dev) override
+ {
+ if (dev->isAdvertisingService(NimBLEUUID((uint16_t)0xFFF0)))
+ {
+ const int rssi = dev->getRSSI();
+ if (rssi > s_bestRssi)
+ {
+ s_bestRssi = rssi;
+ s_foundAddr = dev->getAddress();
+ }
+ s_found = true;
+ }
+ }
+};
+
+class TrainerClientCb : public NimBLEClientCallbacks
+{
+ void onDisconnect(NimBLEClient *) override
+ {
+ s_connected = false;
+ }
+};
+
+static bool subscribeTrainer()
+{
+ NimBLERemoteService *svc = s_client->getService(NimBLEUUID((uint16_t)0xFFF0));
+ if (!svc)
+ return false;
+ NimBLERemoteCharacteristic *ch = svc->getCharacteristic(NimBLEUUID((uint16_t)0xFFF6));
+ if (!ch || !ch->canNotify())
+ return false;
+ ch->subscribe(true, onNotify);
+ return true;
+}
+
+static void bleTask(void *)
+{
+ for (;;)
+ {
+ if (s_forget)
+ {
+ s_forget = false;
+ config.ClearTrainerPeerMac();
+ config.Commit();
+ if (s_client->isConnected())
+ s_client->disconnect();
+ s_connected = false;
+ }
+
+ if (!s_connected)
+ {
+ bool ok = false;
+ if (config.IsTrainerPaired())
+ {
+ NimBLEAddress addr(config.GetTrainerPeerMac(), config.GetTrainerPeerType());
+ ok = s_client->connect(addr);
+ if (ok)
+ {
+ s_failCount = 0;
+ }
+ else if (++s_failCount >= 2)
+ {
+ // Peer unreachable: drop it and go back to pairing mode.
+ s_failCount = 0;
+ config.ClearTrainerPeerMac();
+ config.Commit();
+ }
+ }
+ else
+ {
+ // Only scan while there is no paired peer (pairing mode).
+ s_found = false;
+ s_bestRssi = -127;
+ s_scan->start(3, false);
+ if (s_found)
+ {
+ ok = s_client->connect(s_foundAddr);
+ if (ok)
+ {
+ // Store the address in canonical (MSB-first) order: the
+ // NimBLEAddress(uint8_t[6]) constructor reverses it back.
+ uint8_t mac[6];
+ const uint8_t *raw = s_foundAddr.getNative();
+ for (int i = 0; i < 6; i++) mac[i] = raw[5 - i];
+ config.SetTrainerPeerMac(mac);
+ config.SetTrainerPeerType(s_foundAddr.getType());
+ config.Commit();
+ }
+ }
+ }
+
+ if (ok && subscribeTrainer())
+ {
+ s_gotFrame = false;
+ s_connected = true;
+ // The trainer requests its own parameters from its connection
+ // callback, so reapply ours after that request has landed.
+ vTaskDelay(pdMS_TO_TICKS(TRAINER_CONN_PARAM_DELAY_MS));
+ applyConnParams();
+ }
+ else
+ {
+ if (s_client->isConnected())
+ s_client->disconnect();
+ vTaskDelay(pdMS_TO_TICKS(2000));
+ }
+ }
+ else
+ {
+ vTaskDelay(pdMS_TO_TICKS(500));
+ }
+ }
+}
+
+static void initBle()
+{
+ // When the backpack also runs the MAVLink WiFi AP, give the radio arbiter a
+ // WiFi preference and use a lower BLE TX power: on the Nomad the module's PA
+ // is next to the C3, and the added BLE airtime/power destabilized the AP.
+ if (wifiService == WIFI_SERVICE_MAVLINK_TX)
+ {
+ esp_coex_preference_set(ESP_COEX_PREFER_WIFI);
+ }
+
+ NimBLEDevice::init("ELRS-BP");
+ // Max TX power in normal use: the backpack sits next to the radio's RF module.
+ // Reduce it while sharing the radio with the MAVLink WiFi AP.
+ NimBLEDevice::setPower(wifiService == WIFI_SERVICE_MAVLINK_TX ? ESP_PWR_LVL_P3 : ESP_PWR_LVL_P9);
+ s_scan = NimBLEDevice::getScan();
+ s_scan->setAdvertisedDeviceCallbacks(new TrainerScanCb(), false);
+ s_scan->setActiveScan(true);
+ s_scan->setInterval(0x50);
+ s_scan->setWindow(0x30);
+ s_client = NimBLEDevice::createClient();
+ setInitialConnParams();
+ s_client->setClientCallbacks(new TrainerClientCb(), true);
+ xTaskCreate(bleTask, "bletrainer", 4096, nullptr, 1, nullptr);
+}
+
+void bleTrainerForgetPeer()
+{
+ s_forget = true;
+}
+
+bool bleTrainerConnected()
+{
+ return s_connected;
+}
+
+uint32_t bleTrainerSetPtrCount()
+{
+ return s_setPtrMagic == TRAINER_FX_MAGIC ? s_setPtrCount : 0;
+}
+
+// ------------------------------------------------------------------
+// device_t
+// ------------------------------------------------------------------
+static void initialize()
+{
+}
+static int start()
+{
+ // Keep BLE off for the firmware updater, but allow it alongside the MAVLink
+ // WiFi service. That mode starts WiFi without initializing ESP-NOW.
+ if (connectionState == wifiUpdate && wifiService != WIFI_SERVICE_MAVLINK_TX)
+ return DURATION_NEVER;
+ initBle();
+ return DURATION_IMMEDIATELY;
+}
+
+static int event()
+{
+ return DURATION_IGNORE;
+}
+
+static int timeout()
+{
+ static uint32_t lastSend = 0;
+
+ // Republish the latest channels at a steady rate so EdgeTX's trainer input
+ // does not time out between (irregular) BLE notifications. The module only
+ // forwards a CRSF channel frame per received SET_PTR packet.
+ // The backpack restarts when MAVLink WiFi is selected, but the TX module
+ // can retain its HT-enable state and therefore not resend the edge-triggered
+ // enable MSP. In that service, forward BLE data and let the TX module enforce
+ // its own enable/override state.
+ const bool trainerOutputEnabled = headTrackingEnabled || wifiService == WIFI_SERVICE_MAVLINK_TX;
+ if (config.GetTrainerSource() != TRAINER_SOURCE_ESPNOW && trainerOutputEnabled && s_connected &&
+ s_gotFrame && (millis() - lastSend >= 20))
+ {
+ lastSend = millis();
+
+ mspPacket_t packet;
+ packet.reset();
+ packet.makeCommand();
+ packet.function = MSP_ELRS_BACKPACK_SET_PTR;
+ for (int i = 0; i < FRSKY_CHANNELS; i++)
+ {
+ const uint16_t pwmUs = frskyChannels[i];
+ uint16_t v;
+ if (pwmUs <= FRSKY_PWM_MIN_US)
+ v = FRSKY_CRSF_MIN_VALUE;
+ else if (pwmUs >= FRSKY_PWM_MAX_US)
+ v = FRSKY_CRSF_MAX_VALUE;
+ else
+ {
+ const uint32_t spanUs = FRSKY_PWM_MAX_US - FRSKY_PWM_MIN_US;
+ const uint32_t spanCrsf = FRSKY_CRSF_MAX_VALUE - FRSKY_CRSF_MIN_VALUE;
+ v = FRSKY_CRSF_MIN_VALUE +
+ ((uint32_t)(pwmUs - FRSKY_PWM_MIN_US) * spanCrsf + spanUs / 2) / spanUs;
+ }
+ packet.addByte(v & 0xFF);
+ packet.addByte(v >> 8);
+ }
+ msp.sendPacket(&packet, &Serial);
+ setPtrTick();
+ }
+ return DURATION_IMMEDIATELY;
+}
+
+device_t BleTrainer_device = {
+ .initialize = initialize,
+ .start = start,
+ .event = event,
+ .timeout = timeout
+};
+
+#endif // HAS_BLE_TRAINER
diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp
index c37911fc..6b683407 100644
--- a/src/Tx_main.cpp
+++ b/src/Tx_main.cpp
@@ -21,6 +21,7 @@
#include "devWIFI.h"
#include "devButton.h"
#include "devLED.h"
+#include "BleTrainer.h"
#if defined(MAVLINK_ENABLED)
#include
@@ -40,12 +41,19 @@ unsigned long rebootTime = 0;
bool cacheFull = false;
bool sendCached = false;
+// Trainer enabled by the radio (MSP_ELRS_BACKPACK_SET_HEAD_TRACKING).
+// When set, a BLE-connected trainer is forwarded to the module as SET_PTR.
+bool headTrackingEnabled = false;
+
device_t *ui_devices[] = {
#ifdef PIN_LED
&LED_device,
#endif
#ifdef PIN_BUTTON
&Button_device,
+#endif
+#ifdef HAS_BLE_TRAINER
+ &BleTrainer_device,
#endif
&WIFI_device,
};
@@ -102,6 +110,13 @@ void ProcessMSPPacketFromPeer(mspPacket_t *packet)
}
case MSP_ELRS_BACKPACK_SET_PTR: {
DBGLN("MSP_ELRS_BACKPACK_SET_PTR...");
+#if defined(HAS_BLE_TRAINER)
+ // BLE is the active source when explicitly selected, or (in Auto) while connected.
+ const trainer_source_t trainerSrc = config.GetTrainerSource();
+ if (trainerSrc == TRAINER_SOURCE_BLE ||
+ (trainerSrc == TRAINER_SOURCE_AUTO && bleTrainerConnected()))
+ break;
+#endif
msp.sendPacket(packet, &Serial);
break;
}
@@ -163,29 +178,45 @@ void HandleConfigMsg(mspPacket_t *packet)
switch (key)
{
case MSP_ELRS_BACKPACK_CONFIG_TLM_MODE:
+ {
+ telem_mode_t newMode = config.GetTelemMode();
+ wifi_service_t newService = config.GetWiFiService();
+ bool newStartWiFi = config.GetStartWiFiOnBoot();
switch (value)
{
case BACKPACK_TELEM_MODE_OFF:
- config.SetTelemMode(BACKPACK_TELEM_MODE_OFF);
- config.SetWiFiService(WIFI_SERVICE_UPDATE);
- config.SetStartWiFiOnBoot(false);
- config.Commit();
+ newMode = BACKPACK_TELEM_MODE_OFF;
+ newService = WIFI_SERVICE_UPDATE;
+ newStartWiFi = false;
break;
case BACKPACK_TELEM_MODE_ESPNOW:
- config.SetTelemMode(BACKPACK_TELEM_MODE_ESPNOW);
- config.SetWiFiService(WIFI_SERVICE_UPDATE);
- config.SetStartWiFiOnBoot(false);
- config.Commit();
+ newMode = BACKPACK_TELEM_MODE_ESPNOW;
+ newService = WIFI_SERVICE_UPDATE;
+ newStartWiFi = false;
break;
case BACKPACK_TELEM_MODE_WIFI:
- config.SetTelemMode(BACKPACK_TELEM_MODE_WIFI);
- config.SetWiFiService(WIFI_SERVICE_MAVLINK_TX);
- config.SetStartWiFiOnBoot(true);
- config.Commit();
+ newMode = BACKPACK_TELEM_MODE_WIFI;
+ newService = WIFI_SERVICE_MAVLINK_TX;
+ newStartWiFi = true;
break;
}
+
+ // The TX module can resend this config. Rebooting on an unchanged value
+ // would restart the backpack in a loop and constantly drop the trainer.
+ if (newMode == config.GetTelemMode() &&
+ newService == config.GetWiFiService() &&
+ newStartWiFi == config.GetStartWiFiOnBoot())
+ {
+ break;
+ }
+
+ config.SetTelemMode(newMode);
+ config.SetWiFiService(newService);
+ config.SetStartWiFiOnBoot(newStartWiFi);
+ config.Commit();
rebootTime = millis();
break;
+ }
}
}
@@ -218,6 +249,7 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
case MSP_ELRS_BACKPACK_SET_HEAD_TRACKING:
DBGLN("Processing MSP_ELRS_BACKPACK_SET_HEAD_TRACKING...");
+ headTrackingEnabled = (packet->payloadSize > 0) && (packet->payload[0] != 0);
cachedHTPacket = *packet;
cacheFull = true;
sendMSPViaEspnow(packet);
@@ -229,7 +261,7 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
{
sendMSPViaWiFiUDP(packet);
}
- if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF)
+ else if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF)
{
sendMSPViaEspnow(packet);
}
@@ -270,6 +302,12 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
void sendMSPViaEspnow(mspPacket_t *packet)
{
+ // WiFi telemetry mode boots without initializing ESP-NOW.
+ if (config.GetTelemMode() == BACKPACK_TELEM_MODE_WIFI)
+ {
+ return;
+ }
+
uint8_t packetSize = msp.getTotalPacketSize(packet);
uint8_t nowDataOutput[packetSize];
@@ -325,12 +363,28 @@ void SendCachedMSP()
}
}
-void SetSoftMACAddress()
+static void InitializeBackpackUID()
{
if (!firmwareOptions.hasUID)
{
memcpy(firmwareOptions.uid, config.GetGroupAddress(), 6);
}
+#if defined(PLATFORM_ESP32)
+ static const uint8_t unboundAddress[6] = {0, 0, 0, 0, 0, 0};
+ if (memcmp(firmwareOptions.uid, unboundAddress, sizeof(unboundAddress)) == 0)
+ {
+ const uint64_t efuseMac = ESP.getEfuseMac();
+ for (uint8_t i = 0; i < 6; i++)
+ firmwareOptions.uid[i] = (uint8_t)(efuseMac >> (8 * (5 - i)));
+ }
+#endif
+ // MAC addresses assigned to the WiFi STA interface must be unicast.
+ firmwareOptions.uid[0] &= ~0x01;
+}
+
+void SetSoftMACAddress()
+{
+ InitializeBackpackUID();
DBG("EEPROM MAC = ");
for (int i = 0; i < 6; i++)
{
@@ -339,9 +393,6 @@ void SetSoftMACAddress()
}
DBGLN(""); // Extra line for serial output readability
- // MAC address can only be set with unicast, so first byte must be even, not odd
- firmwareOptions.uid[0] = firmwareOptions.uid[0] & ~0x01;
-
WiFi.mode(WIFI_STA);
#if defined(PLATFORM_ESP8266)
WiFi.setOutputPower(20.5);
@@ -391,6 +442,20 @@ void setup()
eeprom.Begin();
config.SetStorageProvider(&eeprom);
config.Load();
+ InitializeBackpackUID();
+
+#if defined(MAVLINK_ENABLED)
+ // The MAVLink WiFi service is a persistent mode selected by the telemetry
+ // setting, unlike the one-shot updater. Restore it on every boot so that
+ // entering the updater (which overwrites the WiFi service) does not leave the
+ // backpack in normal mode with no access point.
+ if (config.GetTelemMode() == BACKPACK_TELEM_MODE_WIFI &&
+ !(config.GetStartWiFiOnBoot() && config.GetWiFiService() == WIFI_SERVICE_UPDATE))
+ {
+ config.SetWiFiService(WIFI_SERVICE_MAVLINK_TX);
+ config.SetStartWiFiOnBoot(true);
+ }
+#endif
devicesInit(ui_devices, ARRAY_SIZE(ui_devices));
@@ -458,6 +523,7 @@ void loop()
devicesUpdate(now);
+
#if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
// If the reboot time is set and the current time is past the reboot time then reboot.
if (rebootTime != 0 && now > rebootTime) {
diff --git a/targets/txbp_esp.ini b/targets/txbp_esp.ini
index 4e5b2c33..8c8cf80d 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@^1.4.1
build_flags =
${env_common_esp32c3.build_flags}
${tx_backpack_common.build_flags}
-D MAVLINK_ENABLED=1
+ -D HAS_BLE_TRAINER=1
-D PIN_BUTTON=9
-D PIN_LED=8
upload_resetmethod = nodemcu