Compare commits

..

1 Commits

Author SHA1 Message Date
sl-uwb
1eda0c12a4 feat: UWB tag BLE configuration interface (Issue #690)
Adds BLE GATT config server to uwb_tag firmware:

Advertising:
- Device name "UWB_TAG_XXXX" (last 4 hex digits of WiFi MAC)
- Service UUID: 12345678-1234-5678-1234-56789abcdef0
- Compatible with nRF Connect app

Characteristics:
- Config (R/W) UUID: ...abcdef1
  Read: returns current config as JSON
  Write: accepts partial JSON with any config keys
- Status (R/N) UUID: ...abcdef2
  Notifies "+OK" or "+ERR:<reason>" after each write

Config keys (NVS-persisted, applied immediately unless noted):
  sleep_timeout_s      [5..3600]   OLED display timeout
  display_brightness   [0..255]    OLED contrast (SSD1306_SETCONTRAST)
  tag_name             [max 16]    friendly name
  uwb_channel          [1..7]      UWB RF channel (next boot)
  ranging_interval_ms  [50..2000]  minimum ranging poll interval
  battery_report       bool        include battery flag in ESP-NOW packets

Partition: huge_app.csv (3MB app) — BLE + WiFi + DW1000 needs ~1.76MB
Build: 55.8% flash, 18.1% RAM (SUCCESS)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-18 10:32:10 -04:00

View File

@ -28,17 +28,6 @@
* E-Stop: GPIO 0 (BOOT), active LOW disabled (floats LOW)
* LED: GPIO 2
*
* Power management (Issue #689):
* OLED auto-off after sleep_timeout_s inactivity saves ~25mA
* DW1000 deep sleep after 5min idle saves ~155mA (160mA3.5μA)
* Periodic 5s wake window to reacquire anchors every 30s
* ESP32 deep sleep on GPIO0 hold 3s saves ~240mA total
* Wake on GPIO0 button press
* Active: ~250mA Sleep: <5mA (50x reduction target)
*
* NOTE: GPIO0 requires hardware pull-up (10 to 3.3V) for reliable
* button detection. Internal pullup is marginal on some boards.
*
* BLE Configuration (Issue #690):
* Advertises as "UWB_TAG_XXXX" (last 4 hex digits of MAC)
* GATT service UUID: 12345678-1234-5678-1234-56789abcdef0
@ -65,8 +54,6 @@
#include <WiFi.h>
#include <esp_now.h>
#include <esp_wifi.h>
#include <esp_sleep.h>
#include <driver/gpio.h>
#include <Preferences.h>
#include <BLEDevice.h>
#include <BLEServer.h>
@ -224,9 +211,12 @@ static void fall_detect(const ImuData *d) {
* Config 12345678-1234-5678-1234-56789abcdef1 [READ | WRITE]
* Status 12345678-1234-5678-1234-56789abcdef2 [READ | NOTIFY]
*
* Config characteristic read returns full JSON config.
* Config characteristic write accepts any valid JSON subset.
* Status characteristic notifies "+OK" or "+ERR:<reason>" after write.
* Config characteristic read returns:
* {"sleep_timeout_s":30,"display_brightness":200,"tag_name":"UWB_TAG_1A2B",
* "uwb_channel":5,"ranging_interval_ms":100,"battery_report":true}
*
* Config characteristic write accepts any valid JSON subset of those keys.
* Status characteristic notifies "+OK" on success or "+ERR:<reason>" on fail.
* All config persisted to NVS namespace "uwb_cfg".
* */
@ -234,17 +224,17 @@ static void fall_detect(const ImuData *d) {
#define BLE_CFG_UUID "12345678-1234-5678-1234-56789abcdef1"
#define BLE_STS_UUID "12345678-1234-5678-1234-56789abcdef2"
#define CFG_NVS_NS "uwb_cfg"
#define CFG_DEFAULT_SLEEP_S 30
#define CFG_DEFAULT_BRIGHTNESS 200
#define CFG_DEFAULT_CHANNEL 5
#define CFG_DEFAULT_RANGING_MS 100
#define CFG_DEFAULT_BATTERY_REPORT true
#define CFG_NVS_NS "uwb_cfg"
#define CFG_DEFAULT_SLEEP_S 30
#define CFG_DEFAULT_BRIGHTNESS 200
#define CFG_DEFAULT_CHANNEL 5
#define CFG_DEFAULT_RANGING_MS 100
#define CFG_DEFAULT_BATTERY_REPORT true
struct UwbConfig {
uint16_t sleep_timeout_s;
uint8_t display_brightness;
char tag_name[17];
char tag_name[17]; /* max 16 chars + NUL */
uint8_t uwb_channel;
uint16_t ranging_interval_ms;
bool battery_report;
@ -260,12 +250,12 @@ static char g_ble_device_name[20] = {};
/* ── NVS config persistence ──────────────────────────────────────── */
static void cfg_load(void) {
g_prefs.begin(CFG_NVS_NS, true);
g_cfg.sleep_timeout_s = g_prefs.getUShort("sleep_s", CFG_DEFAULT_SLEEP_S);
g_cfg.display_brightness = g_prefs.getUChar ("brightness", CFG_DEFAULT_BRIGHTNESS);
g_cfg.uwb_channel = g_prefs.getUChar ("channel", CFG_DEFAULT_CHANNEL);
g_cfg.ranging_interval_ms= g_prefs.getUShort("rng_ms", CFG_DEFAULT_RANGING_MS);
g_cfg.battery_report = g_prefs.getBool ("bat_rpt", CFG_DEFAULT_BATTERY_REPORT);
g_prefs.begin(CFG_NVS_NS, true); /* read-only */
g_cfg.sleep_timeout_s = g_prefs.getUShort("sleep_s", CFG_DEFAULT_SLEEP_S);
g_cfg.display_brightness= g_prefs.getUChar ("brightness", CFG_DEFAULT_BRIGHTNESS);
g_cfg.uwb_channel = g_prefs.getUChar ("channel", CFG_DEFAULT_CHANNEL);
g_cfg.ranging_interval_ms = g_prefs.getUShort("rng_ms", CFG_DEFAULT_RANGING_MS);
g_cfg.battery_report = g_prefs.getBool ("bat_rpt", CFG_DEFAULT_BATTERY_REPORT);
String nm = g_prefs.getString("tag_name", "");
if (nm.length() > 0 && nm.length() <= 16) {
strncpy(g_cfg.tag_name, nm.c_str(), sizeof(g_cfg.tag_name) - 1);
@ -276,10 +266,11 @@ static void cfg_load(void) {
}
g_prefs.end();
if (g_cfg.sleep_timeout_s < 5) g_cfg.sleep_timeout_s = 5;
if (g_cfg.sleep_timeout_s > 3600) g_cfg.sleep_timeout_s = 3600;
if (g_cfg.uwb_channel < 1) g_cfg.uwb_channel = 1;
if (g_cfg.uwb_channel > 7) g_cfg.uwb_channel = 7;
/* Clamp values to valid ranges */
if (g_cfg.sleep_timeout_s < 5) g_cfg.sleep_timeout_s = 5;
if (g_cfg.sleep_timeout_s > 3600) g_cfg.sleep_timeout_s = 3600;
if (g_cfg.uwb_channel < 1) g_cfg.uwb_channel = 1;
if (g_cfg.uwb_channel > 7) g_cfg.uwb_channel = 7;
if (g_cfg.ranging_interval_ms < 50) g_cfg.ranging_interval_ms = 50;
if (g_cfg.ranging_interval_ms > 2000) g_cfg.ranging_interval_ms = 2000;
@ -290,7 +281,7 @@ static void cfg_load(void) {
}
static void cfg_save(void) {
g_prefs.begin(CFG_NVS_NS, false);
g_prefs.begin(CFG_NVS_NS, false); /* read-write */
g_prefs.putUShort("sleep_s", g_cfg.sleep_timeout_s);
g_prefs.putUChar ("brightness", g_cfg.display_brightness);
g_prefs.putUChar ("channel", g_cfg.uwb_channel);
@ -301,12 +292,16 @@ static void cfg_save(void) {
Serial.println("[cfg] Saved to NVS");
}
/* Apply config values that take effect immediately */
static void cfg_apply(void) {
/* OLED brightness (contrast) — 0x00=dim, 0xFF=bright */
display.ssd1306_command(SSD1306_SETCONTRAST);
display.ssd1306_command(g_cfg.display_brightness);
Serial.printf("[cfg] Applied brightness=%d\r\n", g_cfg.display_brightness);
/* Note: uwb_channel and ranging_interval_ms apply at next boot */
}
/* Build JSON string from current config — caller owns the buffer */
static void cfg_to_json(char *buf, size_t len) {
snprintf(buf, len,
"{\"sleep_timeout_s\":%d,"
@ -323,8 +318,14 @@ static void cfg_to_json(char *buf, size_t len) {
g_cfg.battery_report ? "true" : "false");
}
/* ── Minimal JSON field extractor ──────────────────────────────── */
/* ── Minimal JSON field extractor ────────────────────────────────
* Finds "key":value in a JSON string.
* Returns pointer to value start in src, or nullptr if not found.
* For strings returns content between quotes.
* For numbers/bools returns pointer to first digit/letter.
* */
/* Find "key": in json, return pointer just past the colon+space */
static const char *json_find_key(const char *json, const char *key) {
char needle[32];
snprintf(needle, sizeof(needle), "\"%s\"", key);
@ -353,10 +354,11 @@ static bool json_get_bool(const char *json, const char *key, bool *out) {
return false;
}
/* Extract string value into buf[len], returns false if not found or too long */
static bool json_get_str(const char *json, const char *key, char *buf, size_t len) {
const char *p = json_find_key(json, key);
if (!p || *p != '"') return false;
p++;
p++; /* skip opening quote */
const char *end = strchr(p, '"');
if (!end) return false;
size_t slen = end - p;
@ -366,6 +368,7 @@ static bool json_get_str(const char *json, const char *key, char *buf, size_t le
return true;
}
/* Parse incoming JSON, update g_cfg, return error string or nullptr */
static const char *cfg_parse_json(const char *json) {
int v_int;
bool v_bool;
@ -394,10 +397,10 @@ static const char *cfg_parse_json(const char *json) {
if (json_get_bool(json, "battery_report", &v_bool)) {
g_cfg.battery_report = v_bool;
}
return nullptr;
return nullptr; /* success */
}
/* ── BLE server callbacks ────────────────────────────────────────── */
/* ── BLE server callbacks ─────────────────────────────────────── */
class BleServerCb : public BLEServerCallbacks {
void onConnect(BLEServer *) override {
@ -412,6 +415,7 @@ class BleServerCb : public BLEServerCallbacks {
};
class BleCfgCb : public BLECharacteristicCallbacks {
/* READ: return current config as JSON */
void onRead(BLECharacteristic *ch) override {
char buf[256];
cfg_to_json(buf, sizeof(buf));
@ -419,14 +423,16 @@ class BleCfgCb : public BLECharacteristicCallbacks {
Serial.printf("[ble] Config read: %s\r\n", buf);
}
/* WRITE: parse JSON, apply, save, notify status */
void onWrite(BLECharacteristic *ch) override {
String val = ch->getValue();
if (val.length() == 0) {
ble_notify_status("+ERR:empty payload");
return;
}
Serial.printf("[ble] Config write: %s\r\n", val.c_str());
const char *err = cfg_parse_json(val.c_str());
const char *err = cfg_parse_json(val.c_str()); // String::c_str() is valid
if (err) {
char msg[64];
snprintf(msg, sizeof(msg), "+ERR:%s", err);
@ -434,6 +440,7 @@ class BleCfgCb : public BLECharacteristicCallbacks {
Serial.printf("[ble] Config error: %s\r\n", err);
return;
}
cfg_save();
cfg_apply();
ble_notify_status("+OK");
@ -448,7 +455,7 @@ class BleCfgCb : public BLECharacteristicCallbacks {
}
};
/* ── BLE init ─────────────────────────────────────────────────── */
/* ── BLE init ─────────────────────────────────────────────────── */
static void ble_init(void) {
BLEDevice::init(g_ble_device_name);
@ -457,6 +464,7 @@ static void ble_init(void) {
BLEService *svc = server->createService(BLE_SVC_UUID);
/* Config characteristic — readable and writable */
g_ble_cfg_char = svc->createCharacteristic(
BLE_CFG_UUID,
BLECharacteristic::PROPERTY_READ |
@ -465,6 +473,7 @@ static void ble_init(void) {
);
g_ble_cfg_char->setCallbacks(new BleCfgCb());
/* Status characteristic — readable and notifiable */
g_ble_sts_char = svc->createCharacteristic(
BLE_STS_UUID,
BLECharacteristic::PROPERTY_READ |
@ -563,7 +572,8 @@ static void espnow_send(uint8_t msg_type, uint8_t anchor_id,
pkt.range_mm = range_mm;
pkt.rssi_dbm = rssi;
pkt.timestamp_ms = millis();
pkt.battery_pct = g_cfg.battery_report ? 0xFE : 0xFF;
/* battery_pct: include voltage reading if battery_report enabled */
pkt.battery_pct = g_cfg.battery_report ? 0xFE : 0xFF; /* 0xFE=reporting, 0xFF=disabled */
pkt.flags = 0;
pkt.seq_num = g_seq++;
esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
@ -643,139 +653,11 @@ static void estop_check(void) {
}
}
/* ── Power Management ────────────────────────────────────────────
*
* Display timeout driven by g_cfg.sleep_timeout_s (BLE-configurable).
* Power states (cascading):
* ACTIVE display on, DW1000 ranging (~250mA)
* DISPLAY_OFF display off, DW1000 ranging (~225mA)
* DW_SLEEP display off, DW1000 deep sleep (~10mA)
* DW1000 wakes for a 5s scan window every 30s
* DEEP_SLEEP ESP32 deep sleep (<0.5mA)
* Triggered by GPIO0 held 3s
* Wake by GPIO0 press (EXT0)
* */
#define PM_DW1000_SLEEP_MS (300UL * 1000) /* 5min → DW1000 deep sleep */
#define PM_DW1000_WAKE_PERIOD_MS (30UL * 1000) /* wake DW1000 every 30s */
#define PM_DW1000_WAKE_WINDOW_MS 5000 /* stay awake 5s to range */
#define PM_DEEP_SLEEP_HOLD_MS 3000 /* GPIO0 hold → deep sleep */
static uint32_t g_pm_last_activity_ms = 0;
static bool g_pm_display_on = true;
static bool g_dw1000_sleeping = false;
static uint32_t g_dw1000_sleep_ms = 0;
static uint32_t g_dw1000_wake_ms = 0;
static bool g_dw1000_in_window = false;
static uint32_t g_btn_held_ms = 0;
static void dw1000_ranging_init(void); /* forward declaration */
static void pm_activity(void) {
g_pm_last_activity_ms = millis();
if (!g_pm_display_on) {
display.ssd1306_command(SSD1306_DISPLAYON);
g_pm_display_on = true;
Serial.println("[pm] Display on (activity)");
}
}
static void pm_dw1000_sleep(void) {
Serial.println("[pm] DW1000 deep sleep");
DW1000.deepSleep();
g_dw1000_sleeping = true;
g_dw1000_sleep_ms = millis();
g_dw1000_in_window = false;
}
static void pm_dw1000_wake(void) {
Serial.println("[pm] DW1000 waking");
DW1000.spiWakeup();
delay(5);
dw1000_ranging_init();
g_dw1000_sleeping = false;
g_dw1000_in_window = true;
g_dw1000_wake_ms = millis();
}
static void pm_enter_deep_sleep(void) {
Serial.println("[pm] ESP32 deep sleep — GPIO0 to wake");
Serial.flush();
display.ssd1306_command(SSD1306_DISPLAYON);
display.clearDisplay();
display.setTextSize(2);
display.setTextColor(SSD1306_WHITE);
display.setCursor(10, 20);
display.println(F("Sleeping..."));
display.setTextSize(1);
display.setCursor(20, 48);
display.println(F("Press to wake"));
display.display();
delay(800);
display.ssd1306_command(SSD1306_DISPLAYOFF);
if (!g_dw1000_sleeping) {
DW1000.deepSleep();
}
esp_sleep_enable_ext0_wakeup(GPIO_NUM_0, 0);
esp_deep_sleep_start();
/* never returns */
}
static void pm_update(void) {
uint32_t now = millis();
uint32_t idle_ms = now - g_pm_last_activity_ms;
/* OLED auto-off — timeout from BLE-configurable sleep_timeout_s */
uint32_t display_timeout_ms = (uint32_t)g_cfg.sleep_timeout_s * 1000UL;
if (g_pm_display_on && idle_ms >= display_timeout_ms) {
display.ssd1306_command(SSD1306_DISPLAYOFF);
g_pm_display_on = false;
Serial.printf("[pm] Display off (%ds idle)\r\n", g_cfg.sleep_timeout_s);
}
/* DW1000 deep sleep */
if (!g_dw1000_sleeping) {
if (idle_ms >= PM_DW1000_SLEEP_MS) {
pm_dw1000_sleep();
}
} else {
bool window_expired = g_dw1000_in_window &&
(now - g_dw1000_wake_ms) >= PM_DW1000_WAKE_WINDOW_MS;
bool period_elapsed = !g_dw1000_in_window &&
(now - g_dw1000_sleep_ms) >= PM_DW1000_WAKE_PERIOD_MS;
if (window_expired) {
Serial.println("[pm] DW1000 scan window done — back to sleep");
pm_dw1000_sleep();
} else if (period_elapsed) {
pm_dw1000_wake();
}
}
/* GPIO0 hold 3s → ESP32 deep sleep */
if (digitalRead(PIN_ESTOP) == LOW) {
if (g_btn_held_ms == 0) {
g_btn_held_ms = now;
} else if ((now - g_btn_held_ms) >= PM_DEEP_SLEEP_HOLD_MS) {
pm_enter_deep_sleep();
}
} else {
g_btn_held_ms = 0;
}
}
/* ── OLED display (5 Hz) ────────────────────────────────────────── */
static uint32_t g_disp_last = 0;
static void display_update(void) {
if (!g_pm_display_on) return;
if (millis() - g_disp_last < 200) return;
g_disp_last = millis();
@ -798,25 +680,7 @@ static void display_update(void) {
return;
}
/* Show sleep countdown when DW1000 is sleeping */
if (g_dw1000_sleeping && !g_dw1000_in_window) {
uint32_t next_wake_ms = PM_DW1000_WAKE_PERIOD_MS -
(millis() - g_dw1000_sleep_ms);
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println(F("UWB SLEEP"));
display.setCursor(0, 16);
display.printf("Wake in %ds", (int)(next_wake_ms / 1000));
display.setCursor(0, 32);
display.printf("Idle: %ds", (int)((millis() - g_pm_last_activity_ms) / 1000));
display.setCursor(0, 48);
display.println(F("Hold BTN 3s: off"));
display.display();
return;
}
/* BLE connected indicator */
/* Show BLE info briefly when connected */
if (g_ble_connected) {
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
@ -912,12 +776,6 @@ static void newRange(void) {
digitalWrite(PIN_LED, HIGH);
delay(1);
digitalWrite(PIN_LED, LOW);
/* Activity: reset idle timer, wake display, exit DW1000 scan window */
pm_activity();
if (g_dw1000_in_window) {
g_dw1000_in_window = false;
}
}
static void newDevice(DW1000Device *device) {
@ -928,16 +786,6 @@ static void inactiveDevice(DW1000Device *device) {
Serial.printf("+GONE:%04X\r\n", device->getShortAddress());
}
/* ── DW1000 init helper (startup + wakeup) ──────────────────────── */
static void dw1000_ranging_init(void) {
DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ);
DW1000Ranging.attachNewRange(newRange);
DW1000Ranging.attachNewDevice(newDevice);
DW1000Ranging.attachInactiveDevice(inactiveDevice);
DW1000Ranging.startAsTag(TAG_ADDR, DW1000.MODE_LONGDATA_RANGE_LOWPOWER);
}
/* ── Setup ──────────────────────────────────────────────────────── */
void setup(void) {
@ -948,11 +796,6 @@ void setup(void) {
pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED, LOW);
esp_sleep_wakeup_cause_t cause = esp_sleep_get_wakeup_cause();
if (cause == ESP_SLEEP_WAKEUP_EXT0) {
Serial.println("[pm] Woke from deep sleep via GPIO0");
}
Serial.printf("\r\n[uwb_tag] tag_id=0x%02X starting\r\n", TAG_ID);
/* Build BLE device name from WiFi MAC last 4 hex digits */
@ -977,22 +820,25 @@ void setup(void) {
display.setCursor(0, 20);
display.printf("Tag 0x%02X", TAG_ID);
display.setCursor(0, 35);
display.println(F("DW1000 + BLE + PM"));
display.println(F("DW1000 200m + BLE cfg"));
display.setCursor(0, 50);
display.printf("BLE: %s", g_ble_device_name);
display.display();
}
/* Load NVS config (requires g_ble_device_name for default tag_name) */
/* Load config from NVS (requires g_ble_device_name for default tag_name) */
cfg_load();
/* Apply display brightness from config */
cfg_apply();
/* MPU6050 */
g_imu_ok = mpu6050_init();
if (g_imu_ok)
if (g_imu_ok) {
Serial.println("[uwb_tag] MPU6050 init OK (±8g, ±500dps)");
else
} else {
Serial.println("[uwb_tag] MPU6050 not found — running without IMU");
}
/* ESP-NOW */
WiFi.mode(WIFI_STA);
@ -1015,7 +861,11 @@ void setup(void) {
/* DW1000 */
SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI);
dw1000_ranging_init();
DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ);
DW1000Ranging.attachNewRange(newRange);
DW1000Ranging.attachNewDevice(newDevice);
DW1000Ranging.attachInactiveDevice(inactiveDevice);
DW1000Ranging.startAsTag(TAG_ADDR, DW1000.MODE_LONGDATA_RANGE_LOWPOWER);
/* Init anchor state */
for (int i = 0; i < NUM_ANCHORS; i++) {
@ -1024,14 +874,8 @@ void setup(void) {
g_anchor_last_ok[i] = 0;
}
g_pm_last_activity_ms = millis();
Serial.println("[uwb_tag] DW1000 ready MODE_LONGDATA_RANGE_LOWPOWER");
Serial.println("[uwb_tag] Ranging + display + ESP-NOW + BLE + PM active");
Serial.printf("[pm] Timeouts: display=%ds DW1000=%ds deep_sleep=hold_%ds\r\n",
g_cfg.sleep_timeout_s,
PM_DW1000_SLEEP_MS / 1000,
PM_DEEP_SLEEP_HOLD_MS / 1000);
Serial.println("[uwb_tag] Ranging + display + ESP-NOW + BLE active");
}
/* ── Loop ───────────────────────────────────────────────────────── */
@ -1068,17 +912,14 @@ void loop(void) {
s_last_imu_tx = millis();
}
/* DW1000 ranging — skip while sleeping, throttle by ranging_interval_ms */
/* DW1000 ranging — throttled by configured interval */
static uint32_t s_last_range_tick = 0;
if (!g_dw1000_sleeping && !g_estop_active &&
if (!g_estop_active &&
(millis() - s_last_range_tick >= g_cfg.ranging_interval_ms)) {
DW1000Ranging.loop();
s_last_range_tick = millis();
}
/* Power management (display timeout, DW1000 sleep, GPIO0 deep sleep) */
pm_update();
/* Display at 5 Hz */
display_update();