Enhance BLE OTA reliability with keepalive and supervision timeout

inc/sp140/ble/ble_ids.h

@@ -19,6 +19,28 @@
 #define THROTTLE_VALUE_UUID "50AB3859-9FBF-4D30-BF97-2516EE632FAD"
 #define DEVICE_STATE_UUID "8F80BCF5-B58F-4908-B079-E8AD6F5EE257"
 
+// ESC config relay (phone -> controller -> ESC over CAN). CMD is written by the
+// app (opcode-multiplexed); STATUS is read/notify for the async result.
+// See: powerpack-flash-qc/configs/ESC-Config-Relay-Design.md
+#define ESC_RELAY_CMD_UUID "E5C0C0DE-0001-4A5C-9B21-7E5C0F1A2B30"
+#define ESC_RELAY_STATUS_UUID "E5C0C0DE-0002-4A5C-9B21-7E5C0F1A2B30"
+
+// ESC firmware relay: CTRL is written with FW_START/FW_END/ABORT opcodes and
+// read/notify for flasher status; DATA receives offset-addressed image chunks
+// (write-without-response for throughput). See ESC-Config-Relay-Design.md §4.
+#define ESC_FW_CTRL_UUID "E5C0C0DE-0003-4A5C-9B21-7E5C0F1A2B30"
+#define ESC_FW_DATA_UUID "E5C0C0DE-0004-4A5C-9B21-7E5C0F1A2B30"
+
+// ESC parameter read-all result blob: app writes [offset u32 LE] then reads back
+// up to ~240 bytes of the result blob from that offset (paged fetch).
+#define ESC_PARAM_DATA_UUID "E5C0C0DE-0005-4A5C-9B21-7E5C0F1A2B30"
+
+// ESC relay NOTIFY: the controller pushes status + streams the result blob here,
+// because GATT reads of the config service return null on this stack while
+// notify is reliable. Frames: [0x01]=STATUS[code][phase][detail][len u16],
+// [0x02]=DATA[offset u16][bytes]. See ESC-Config-Relay-Design.md.
+#define ESC_RELAY_NOTIFY_UUID "E5C0C0DE-0006-4A5C-9B21-7E5C0F1A2B30"
+
 // Device info service
 #define DEVICE_INFO_SERVICE_UUID "180A"
 #define MANUFACTURER_NAME_UUID "2A29"

inc/sp140/ble/config_service.h

@@ -8,4 +8,8 @@ class NimBLEServer;
 void initConfigBleService(NimBLEServer* server, const std::string& uniqueId);
 void updateThrottleBLE(int value);
 
+// Push ESC relay status + stream the result blob over notify. Call from the
+// 50 Hz BLE notify task.
+void pumpEscRelayNotify();
+
 #endif  // INC_SP140_BLE_CONFIG_SERVICE_H_

inc/sp140/bms.h

@@ -18,6 +18,47 @@
 constexpr uint8_t BMS_CELL_PROBE_COUNT = 4;
 constexpr uint8_t BMS_MAX_IGNORED_DISCONNECTED_PROBES = 2;
 
+// How long the BMS link may go silent before it counts as disconnected. Must
+// stay well clear of the bmsTask poll period (100 ms): with the library's 100 ms
+// default there is zero timing margin, so a single skipped CAN drain — e.g. the
+// SPI-mutex bail in updateBMSData() — publishes a spurious disconnect, which
+// force-clears every BMS alert.
+constexpr unsigned long BMS_LINK_TIMEOUT_MS = 500;
+
+// A just-connected BMS spreads its state across several CAN frames (basic
+// info 1: pack voltage/SOC/current, basic info 2: cell voltages, 0x18B4:
+// temperatures). Treating the link as CONNECTED on the first frame let
+// monitors and the UI see zero-initialized fields — a 0.000 V "highest cell"
+// fired a critical displayed as cell-voltage-high at every boot. Only report
+// CONNECTED once both basic frames have populated the snapshot.
+//
+// Deliberately tests HIGHEST cell voltage, not lowest: both are written by the
+// same frame (0x18FE28F4), so either proves that frame arrived — but a shorted
+// cell or broken sense lead drives the LOWEST cell to 0.000 V on a healthy,
+// still-transmitting pack. Keying on lowest would read that catastrophic fault
+// as "no data" and drop the whole BMS to NOT_CONNECTED, suppressing the very
+// low-cell and voltage-differential alerts that exist to catch it. No
+// single-cell fault can pull the highest cell of a live pack below this floor.
+// Callers must latch the result (see bmsTask) so this stays a boot-ordering
+// gate and never becomes a runtime state.
+inline bool bmsSnapshotCoherent(const STR_BMS_TELEMETRY_140& t) {
+  return t.battery_voltage > 5.0f &&     // basic info 1 received
+         t.highest_cell_voltage > 0.5f;  // basic info 2 received
+}
+
+// True once at least one reading from the BMS temperature frame has been
+// parsed. Before that frame arrives every probe reads NaN — indistinguishable
+// from "all probes disconnected" — and the disconnect sentinel policy below
+// must not run, or it fires -40 °C criticals for T3/T4 at every boot.
+inline bool bmsTempFrameSeen(float mosTemp, float balanceTemp,
+                             const float cellTemps[BMS_CELL_PROBE_COUNT]) {
+  if (!isnan(mosTemp) || !isnan(balanceTemp)) return true;
+  for (uint8_t i = 0; i < BMS_CELL_PROBE_COUNT; i++) {
+    if (!isnan(cellTemps[i])) return true;
+  }
+  return false;
+}
+
 inline void sanitizeCellProbeTemps(
     const float temps[BMS_CELL_PROBE_COUNT],
     float out[BMS_CELL_PROBE_COUNT]) {

inc/sp140/esc.h

@@ -30,6 +30,11 @@ void setESCThrottle(int throttlePWM);
 void readESCTelemetry();
 bool setupTWAI();
 
+// Accessor to the single shared CanardAdapter (the controller's sole CAN owner).
+// Used by the ESC config relay so its requests go out on the same adapter the
+// throttle task drives — keeping all CAN traffic on one task, no mutex needed.
+CanardAdapter& escAdapter();
+
 // Request ESC hardware info (HW ID, FW version, bootloader, serial number).
 // Thread-safe: sets a flag consumed by readESCTelemetry() on its next tick.
 // Also called automatically the first time the ESC connects.

inc/sp140/esc_config_relay.h

@@ -0,0 +1,145 @@
+#ifndef INC_SP140_ESC_CONFIG_RELAY_H_
+#define INC_SP140_ESC_CONFIG_RELAY_H_
+
+#include <stdint.h>
+
+// =============================================================================
+// ESC Configuration Relay
+//
+// Relays SINE / Mad Motors ESC parameter-config commands from the phone app
+// (over BLE) to the ESC (over CAN), using the proven SetConfig / SaveConfig /
+// RestartNode / GetConfig sequence ported from powerpack-flash-qc.
+//
+// Concurrency model: the controller's single CAN owner is the throttle task
+// (readESCTelemetry() -> adapter.processTxRxOnce()). All CAN traffic for a
+// relay session therefore runs on that task via escConfigRelayServiceTick().
+// The BLE task only ENQUEUES a request (escConfigRelayRequest*), exactly like
+// the existing requestEscHardwareInfo() flag handoff in esc.cpp. The CanardAdapter
+// is never touched from two tasks, so no mutex is needed on it.
+//
+// Safety: a session only starts while the device is DISARMED. The session is
+// fully non-blocking — it never calls delay()/vTaskDelay(), so it cannot stall
+// the 50 Hz control loop. Only the ESC reboots; the controller does not.
+//
+// See: powerpack-flash-qc/configs/ESC-Config-Relay-Design.md
+// =============================================================================
+
+// Canonical "reverse motor direction" parameter (SINE config_id 0x0080, UINT16,
+// 0 = positive/normal, 1 = inversion/reversed). It is a "basic" parameter,
+// documented as accessible WITHOUT the password — see
+// powerpack-flash-qc/configs/ESC-CAN-Config-Protocol.md.
+static const uint16_t ESC_PARAM_DIRECTION = 0x0080;
+
+enum class EscRelayPhase : uint8_t {
+  IDLE = 0,
+  UNLOCK,
+  WRITE,
+  SAVE,
+  RESTART,
+  WAIT_REBOOT,
+  VERIFY,
+  READING,      // read-all: iterating GetConfig over every param
+  BATCH_WRITE,  // batch: iterating SetConfig over every queued param
+  BATCH_VERIFY,  // batch: iterating GetConfig to confirm each queued param persisted
+  DONE_OK,
+  DONE_FAIL,
+};
+
+// Status codes surfaced to the app (ESC_RELAY_STATUS notify byte 0 in the
+// design doc).
+enum class EscRelayStatusCode : uint8_t {
+  IDLE            = 0x00,
+  ACCEPTED        = 0x01,
+  RUNNING         = 0x02,
+  REBOOTING       = 0x03,
+  VERIFIED_OK     = 0x04,
+  READING         = 0x06,  // read-all in progress (detail = percent)
+  READ_DONE       = 0x07,  // read-all complete (config_id field = result blob length)
+  FAILED          = 0x80,
+  REJECTED_ARMED  = 0x81,
+  TIMEOUT         = 0x82,
+  VERIFY_MISMATCH = 0x83,
+  ESC_FLAG_ERROR  = 0x85,
+};
+
+struct EscRelayStatus {
+  EscRelayStatusCode code;
+  EscRelayPhase phase;
+  uint16_t config_id;
+  uint8_t detail;          // raw ESC flag byte on failure, else 0
+  uint8_t readback[8];     // verified read-back bytes (valid on VERIFIED_OK)
+  uint8_t readback_len;
+};
+
+// Call once from initESC() (after the CanardAdapter has been begun).
+void escConfigRelayInit();
+
+// Drive the session state machine. MUST be called from the throttle task — it
+// is invoked from readESCTelemetry(). Non-blocking.
+void escConfigRelayServiceTick();
+
+// ---- Request API (safe to call from the BLE task) --------------------------
+// Returns false if a session is already in progress, the payload is too large,
+// or the relay is not initialized. The DISARMED check is (re)enforced on the
+// throttle task before the session actually starts.
+//
+// Writes a single parameter, persists it (SaveConfig), restarts the ESC, then
+// verifies the read-back across the reboot (GetConfig byte-compare).
+bool escConfigRelayRequestSetParam(uint16_t config_id,
+                                   const uint8_t* data, uint8_t len);
+
+// Convenience for the canonical reverse-direction toggle.
+bool escConfigRelayRequestReverseDirection(bool reversed);
+
+// Start a "read all parameters" session: switch to host node 0x40, unlock, then
+// GetConfig every id in ESC_PARAM_IDS into a result blob, then restore node 0x01.
+// On completion the status code is READ_DONE and config_id holds the blob length.
+// Returns false if a session is already in progress.
+bool escConfigRelayRequestReadAll();
+
+// ---- Batch write (apply many params with a SINGLE save + restart) -----------
+// Mirrors the flash-qc sequence: unlock -> SetConfig every queued param ->
+// SaveConfig once -> RestartNode once -> verify each param persisted across the
+// reboot. The phone streams the batch first (Begin, then Add per param) and then
+// Commit triggers the session. Safe to call Begin/Add from the BLE task — they
+// only touch the staging buffer, which the throttle task does not read until a
+// Commit starts the session.
+//
+// Begin: clear the staging buffer. Returns false if a session is in progress.
+bool escConfigRelayBatchBegin();
+// Add one param to the staging buffer ([config_id u16][len u8][data[len]]).
+// Returns false if the buffer is full or a session is in progress.
+bool escConfigRelayBatchAdd(uint16_t config_id, const uint8_t* data, uint8_t len);
+// Commit: run the batch session over the staged params. Returns false if nothing
+// is staged or a session is already in progress.
+bool escConfigRelayRequestBatchCommit();
+
+// Length of the last completed read-all result blob (0 until READ_DONE).
+uint16_t escConfigRelayResultLen();
+
+// Copy up to maxLen bytes of the result blob starting at offset into out.
+// Returns the number of bytes copied. Blob format is a sequence of tuples:
+//   [config_id u16 LE][flag u8][len u8][data[len]]
+// (flag 0xFF = the controller timed out reading that param.)
+uint16_t escConfigRelayReadResult(uint32_t offset, uint8_t* out, uint16_t maxLen);
+
+// Latched status snapshot for the BLE status characteristic / UI.
+EscRelayStatus escConfigRelayGetStatus();
+
+// True while a session is in progress OR a request is pending (for the arm
+// interlock — block arming while this is true).
+bool escConfigRelayIsActive();
+
+// True while a session is active OR within a short settle window after it ends.
+// Used to keep the high-rate telemetry notify throttled so the phone's status
+// poll and result-blob reads aren't starved by the notify flood. See
+// fastlink_service.cpp / ESC-Config-Relay-Design.md.
+bool escConfigRelayResultPending();
+
+// Require PasswordUnlock (SINE service 225) before writing, and re-unlock after
+// the ESC reboots. Default false. The reverse-direction param does NOT need it;
+// restricted params do. The decisive bench test runs with this false to confirm
+// that 0x0080 is writable from the controller's node 0x01 with no unlock.
+void escConfigRelaySetRequireUnlock(bool require);
+
+#endif  // INC_SP140_ESC_CONFIG_RELAY_H_

inc/sp140/esc_flasher_relay.h

@@ -0,0 +1,94 @@
+#ifndef INC_SP140_ESC_FLASHER_RELAY_H_
+#define INC_SP140_ESC_FLASHER_RELAY_H_
+
+#include <stdint.h>
+
+// =============================================================================
+// ESC Firmware Relay
+//
+// Relays an ESC firmware image from the phone app (over BLE) to the ESC (over
+// CAN), using the SINE/Mad Motors bootloader protocol (GetBootStatus 200 /
+// StartFwUpgrade 201 / SendFwData 202 / EndFwUpgrade 203) ported from
+// powerpack-flash-qc/src/esc_flasher.cpp.
+//
+// Transport model: BUFFER-THEN-FLASH.
+//   1. The app sends FW_START(hwId, totalSize), then streams the complete image
+//      (32-byte header + firmware) in offset-addressed chunks into a controller
+//      heap/PSRAM buffer (BLE task), then FW_END.
+//   2. Only after the full image is received does the throttle task (the sole
+//      CAN owner) stream it to the ESC bootloader in 256-byte SendFwData chunks,
+//      paced by the ESC's next_index flow control.
+// This decouples the BLE transfer from the CAN streaming entirely, so a slow
+// ESC flash cannot trip a BLE idle timeout, and the buffer is written by only
+// one task at a time (BLE during RECEIVING, throttle during flashing).
+//
+// Safety: only starts while DISARMED; aborts if the device arms; the controller
+// never reboots (only the ESC does); the hardware_id is validated against the
+// connected ESC before entering the bootloader. Fully non-blocking on the
+// throttle task (no delay()/vTaskDelay()), so the 50 Hz control loop is never
+// stalled. See: powerpack-flash-qc/configs/ESC-Config-Relay-Design.md (§4).
+// =============================================================================
+
+enum class EscFwPhase : uint8_t {
+  IDLE = 0,
+  RECEIVING,      // BLE filling the image buffer
+  RESTARTING,     // RestartNode sent, waiting (non-blocking) for ESC reboot
+  CHECKING_BOOT,  // polling GetBootStatus for bootloader mode
+  STARTING,       // StartFwUpgrade sent
+  SENDING,        // streaming SendFwData chunks
+  ENDING,         // EndFwUpgrade sent
+  DONE_OK,
+  DONE_FAIL,
+};
+
+enum class EscFwCode : uint8_t {
+  IDLE           = 0x00,
+  RECEIVING      = 0x01,
+  ENTER_BOOTLDR  = 0x02,
+  FLASHING       = 0x03,
+  SUCCESS        = 0x04,
+  FAILED         = 0x80,
+  REJECTED_ARMED = 0x81,
+  TIMEOUT        = 0x82,
+  REJECTED_HWID  = 0x83,   // image hardware_id != connected ESC
+  REJECTED_BUSY  = 0x84,   // another ESC session active / alloc failed / bad size
+  ESC_REJECTED   = 0x85,   // ESC refused the image (bootloader/Start/End state != 0)
+};
+
+struct EscFwStatus {
+  EscFwCode code;
+  EscFwPhase phase;
+  uint16_t progressPermille;  // 0..1000
+};
+
+// Call once from initESC() (after the CanardAdapter has been begun).
+void escFlasherRelayInit();
+
+// Drive the flasher state machine. MUST be called from the throttle task — it
+// is invoked from readESCTelemetry(). Non-blocking.
+void escFlasherRelayServiceTick();
+
+// ---- BLE-side API (safe to call from the BLE task) -------------------------
+// Begin a transfer: allocate a buffer for totalSize bytes (the complete image
+// incl. 32-byte header) and validate hwId against the connected ESC. Returns
+// false if armed, busy, hwId mismatch, size out of range, or allocation fails.
+bool escFlasherRelayBegin(uint16_t hardwareId, uint32_t totalSize);
+
+// Copy a received chunk into the image buffer at the given offset. Returns the
+// total number of contiguous-from-zero bytes received so far, or -1 on error.
+int32_t escFlasherRelayWriteChunk(uint32_t offset, const uint8_t* data, uint16_t len);
+
+// All bytes sent: validate and hand off to the throttle task to flash. Returns
+// false if the image is incomplete/invalid.
+bool escFlasherRelayEnd();
+
+// Abort and free the buffer.
+void escFlasherRelayAbort();
+
+// Latched status snapshot for the BLE status read/notify.
+EscFwStatus escFlasherRelayGetStatus();
+
+// True while receiving OR flashing (for the arm interlock).
+bool escFlasherRelayIsActive();
+
+#endif  // INC_SP140_ESC_FLASHER_RELAY_H_

inc/sp140/esc_param_ids.h

@@ -0,0 +1,35 @@
+#ifndef INC_SP140_ESC_PARAM_IDS_H_
+#define INC_SP140_ESC_PARAM_IDS_H_
+
+#include <stdint.h>
+
+// The full set of SINE/Mad Motors ESC config_ids to read back during a
+// "read all" session. Ported from powerpack-flash-qc/inc/param_table.h
+// (PRODUCTION_CONFIG order). The controller only needs the id list — GetConfig
+// returns each value as TAO data with its own length; the app holds the catalog
+// (name / type / scaling) for display. See ESC-Config-Relay-Design.md.
+static const uint16_t ESC_PARAM_IDS[] = {
+  0x0001,  // config_name
+  0x0002, 0x0004, 0x0003, 0x0030, 0x0031, 0x0032, 0x0033, 0x0034, 0x0038,
+  0x0040, 0x0041, 0x0042,                                  // board
+  0x0050, 0x0051, 0x0052, 0x0053, 0x0056, 0x0057, 0x0058, 0x0060, 0x0061,  // motor
+  0x0071, 0x0072, 0x0080, 0x0081, 0x0082, 0x0091, 0x0100, 0x0101,          // control
+  0x0110, 0x0111, 0x0112, 0x0118, 0x0119, 0x0120, 0x0121, 0x0130, 0x0131,
+  0x0150, 0x0151, 0x0152, 0x0154, 0x0155, 0x0157, 0x0158, 0x0140, 0x0141,  // speed
+  0x0600, 0x0601, 0x0602, 0x0603, 0x0604, 0x0605, 0x0606, 0x060B, 0x060E,
+  0x060F, 0x0610,                                          // throttle
+  0x0210, 0x0212, 0x0211, 0x0240, 0x0241, 0x0230, 0x0231, 0x0220, 0x0221,
+  0x0250, 0x0251,                                          // advance
+  0x0300, 0x0301, 0x0302,                                  // observer
+  0x0400, 0x0404, 0x0405, 0x0402, 0x0403, 0x0401, 0x0410, 0x0420, 0x0421,
+  0x0422, 0x0423, 0x0424, 0x0425, 0x0426, 0x0381, 0x0035, 0x0036, 0x0382,
+  0x0391, 0x0392, 0x0393, 0x0390,                          // protect
+  0x0607, 0x0608, 0x0609, 0x060A, 0x060C, 0x060D,          // bidirectional throttle
+  0x0180, 0x0181, 0x0182, 0x0183, 0x0184, 0x0185, 0x0186, 0x0187, 0x0188,
+  0x0189, 0x018A, 0x018B, 0x018C, 0x018D,                  // position mode
+};
+
+static const uint16_t ESC_PARAM_IDS_COUNT =
+    sizeof(ESC_PARAM_IDS) / sizeof(ESC_PARAM_IDS[0]);
+
+#endif  // INC_SP140_ESC_PARAM_IDS_H_