From 8ee9b4cca9c983dae149c3205b329412266c6ca7 Mon Sep 17 00:00:00 2001 From: sl-uwb Date: Wed, 18 Mar 2026 10:32:10 -0400 Subject: [PATCH] feat: UWB tag BLE configuration interface (Issue #690) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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:" 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 --- esp32/uwb_tag/platformio.ini | 1 + esp32/uwb_tag/src/main.cpp | 511 ++++++++++++++++++++++++++--------- 2 files changed, 380 insertions(+), 132 deletions(-) diff --git a/esp32/uwb_tag/platformio.ini b/esp32/uwb_tag/platformio.ini index 765e959..2c9f3ad 100644 --- a/esp32/uwb_tag/platformio.ini +++ b/esp32/uwb_tag/platformio.ini @@ -14,6 +14,7 @@ lib_extra_dirs = ../../lib lib_deps = adafruit/Adafruit SSD1306@^2.5.7 adafruit/Adafruit GFX Library@^1.11.5 +board_build.partitions = huge_app.csv build_flags = -DCORE_DEBUG_LEVEL=0 -DTAG_ID=0x01 diff --git a/esp32/uwb_tag/src/main.cpp b/esp32/uwb_tag/src/main.cpp index b6c7ca2..fe48cb4 100644 --- a/esp32/uwb_tag/src/main.cpp +++ b/esp32/uwb_tag/src/main.cpp @@ -29,15 +29,34 @@ * LED: GPIO 2 * * Power management (Issue #689): - * OLED auto-off after 30s inactivity → saves ~25mA + * OLED auto-off after sleep_timeout_s inactivity → saves ~25mA * DW1000 deep sleep after 5min idle → saves ~155mA (160mA→3.5μA) - * Periodic 10s wake window to reacquire anchors + * 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 (10kΩ 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 + * Config characteristic (R/W): 12345678-1234-5678-1234-56789abcdef1 + * Read: returns current config as JSON + * Write: accepts JSON with any subset of config keys + * Status characteristic (R/N): 12345678-1234-5678-1234-56789abcdef2 + * Notifies "+OK" or "+ERR:reason" after a write + * Config persisted in NVS (survives reboot) + * Compatible with nRF Connect app + * + * JSON config keys: + * sleep_timeout_s int [5..3600] OLED sleep timeout (seconds) + * display_brightness int [0..255] OLED contrast + * tag_name str [max 16] friendly name (used in BLE name) + * uwb_channel int [1..7] UWB RF channel (applied next boot) + * ranging_interval_ms int [50..2000] minimum ranging poll interval + * battery_report bool include battery % in ESP-NOW packets */ #include @@ -48,6 +67,11 @@ #include #include #include +#include +#include +#include +#include +#include #include "DW1000Ranging.h" #include @@ -91,25 +115,24 @@ Adafruit_SSD1306 display(128, 64, &Wire, -1); #define MPU6050_ACCEL_CFG 0x1C #define MPU6050_GYRO_CFG 0x1B #define MPU6050_ACCEL_XOUT 0x3B -#define MPU6050_INT_CFG 0x37 /* INT pin config */ -#define MPU6050_INT_EN 0x38 /* Interrupt enable */ -#define MPU6050_MOT_THR 0x1F /* Motion threshold */ -#define MPU6050_MOT_DUR 0x20 /* Motion duration */ +#define MPU6050_INT_CFG 0x37 +#define MPU6050_INT_EN 0x38 +#define MPU6050_MOT_THR 0x1F +#define MPU6050_MOT_DUR 0x20 static bool g_imu_ok = false; struct ImuData { - float ax, ay, az; /* g (±8g range) */ - float gx, gy, gz; /* deg/s (±500 dps range) */ - float accel_mag; /* total acceleration magnitude */ + float ax, ay, az; + float gx, gy, gz; + float accel_mag; }; static ImuData g_imu = {}; -/* Fall detection */ -#define FALL_ACCEL_THRESHOLD 2.5f /* >2.5g = possible fall/impact */ -#define FALL_FREEFALL_LOW 0.3f /* <0.3g = freefall phase */ -#define FALL_COOLDOWN_MS 5000 /* Don't re-trigger for 5s */ +#define FALL_ACCEL_THRESHOLD 2.5f +#define FALL_FREEFALL_LOW 0.3f +#define FALL_COOLDOWN_MS 5000 static bool g_fall_detected = false; static uint32_t g_fall_last_ms = 0; @@ -132,24 +155,16 @@ static uint8_t mpu6050_read_byte(uint8_t reg) { } static bool mpu6050_init(void) { - /* Check WHO_AM_I (should be 0x68 for MPU6050, 0x72 for MPU6050C) */ uint8_t who = mpu6050_read_byte(MPU6050_WHO_AM_I); if (who != 0x68 && who != 0x72 && who != 0x70) { Serial.printf("[imu] WHO_AM_I=0x%02X — not MPU6050\r\n", who); return false; } Serial.printf("[imu] MPU6050 found, WHO_AM_I=0x%02X\r\n", who); - - /* Wake up (clear sleep bit), use gyro X PLL as clock */ mpu6050_write(MPU6050_PWR_MGMT_1, 0x01); delay(10); - - /* Accel range: ±8g (sensitivity = 4096 LSB/g) */ mpu6050_write(MPU6050_ACCEL_CFG, 0x10); - - /* Gyro range: ±500 dps (sensitivity = 65.5 LSB/°/s) */ mpu6050_write(MPU6050_GYRO_CFG, 0x08); - return true; } @@ -163,40 +178,26 @@ static void mpu6050_read(ImuData *d) { for (int i = 0; i < 7; i++) { raw[i] = (Wire.read() << 8) | Wire.read(); } - - /* ±8g → 4096 LSB/g */ d->ax = raw[0] / 4096.0f; d->ay = raw[1] / 4096.0f; d->az = raw[2] / 4096.0f; - /* raw[3] = temperature, skip */ - /* ±500 dps → 65.5 LSB/°/s */ d->gx = raw[4] / 65.5f; d->gy = raw[5] / 65.5f; d->gz = raw[6] / 65.5f; - d->accel_mag = sqrtf(d->ax * d->ax + d->ay * d->ay + d->az * d->az); } -/* Fall detection state machine: - * 1. Freefall phase: accel_mag < 0.3g for >50ms - * 2. Impact phase: accel_mag > 2.5g within 500ms of freefall - * Either a sudden impact alone (>3g) also triggers. - */ static void fall_detect(const ImuData *d) { uint32_t now = millis(); - - /* Cooldown */ if (g_fall_detected && (now - g_fall_last_ms) < FALL_COOLDOWN_MS) return; g_fall_detected = false; - /* Check freefall */ if (d->accel_mag < FALL_FREEFALL_LOW) { if (!g_freefall_phase) { g_freefall_phase = true; g_freefall_start = now; } } else { - /* Check for impact after freefall */ if (g_freefall_phase && (now - g_freefall_start) > 50) { if (d->accel_mag > FALL_ACCEL_THRESHOLD && (now - g_freefall_start) < 500) { @@ -209,7 +210,6 @@ static void fall_detect(const ImuData *d) { g_freefall_phase = false; } - /* Sudden high-g impact without freefall (e.g., side collision) */ if (!g_fall_detected && d->accel_mag > 3.5f) { g_fall_detected = true; g_fall_last_ms = now; @@ -217,6 +217,277 @@ static void fall_detect(const ImuData *d) { } } +/* ── BLE Configuration ─────────────────────────────────────────── + * + * GATT layout: + * Service 12345678-1234-5678-1234-56789abcdef0 + * 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:" after write. + * All config persisted to NVS namespace "uwb_cfg". + * ─────────────────────────────────────────────────────────────── */ + +#define BLE_SVC_UUID "12345678-1234-5678-1234-56789abcdef0" +#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 + +struct UwbConfig { + uint16_t sleep_timeout_s; + uint8_t display_brightness; + char tag_name[17]; + uint8_t uwb_channel; + uint16_t ranging_interval_ms; + bool battery_report; +}; + +static UwbConfig g_cfg = {}; +static Preferences g_prefs; +static BLECharacteristic *g_ble_cfg_char = nullptr; +static BLECharacteristic *g_ble_sts_char = nullptr; +static bool g_ble_connected = false; +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); + 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); + g_cfg.tag_name[sizeof(g_cfg.tag_name) - 1] = '\0'; + } else { + strncpy(g_cfg.tag_name, g_ble_device_name, sizeof(g_cfg.tag_name) - 1); + g_cfg.tag_name[sizeof(g_cfg.tag_name) - 1] = '\0'; + } + 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; + 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; + + Serial.printf("[cfg] Loaded: sleep=%ds bright=%d name=%s ch=%d rng=%dms bat=%d\r\n", + g_cfg.sleep_timeout_s, g_cfg.display_brightness, + g_cfg.tag_name, g_cfg.uwb_channel, + g_cfg.ranging_interval_ms, (int)g_cfg.battery_report); +} + +static void cfg_save(void) { + g_prefs.begin(CFG_NVS_NS, false); + 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); + g_prefs.putUShort("rng_ms", g_cfg.ranging_interval_ms); + g_prefs.putBool ("bat_rpt", g_cfg.battery_report); + g_prefs.putString("tag_name", g_cfg.tag_name); + g_prefs.end(); + Serial.println("[cfg] Saved to NVS"); +} + +static void cfg_apply(void) { + display.ssd1306_command(SSD1306_SETCONTRAST); + display.ssd1306_command(g_cfg.display_brightness); + Serial.printf("[cfg] Applied brightness=%d\r\n", g_cfg.display_brightness); +} + +static void cfg_to_json(char *buf, size_t len) { + snprintf(buf, len, + "{\"sleep_timeout_s\":%d," + "\"display_brightness\":%d," + "\"tag_name\":\"%s\"," + "\"uwb_channel\":%d," + "\"ranging_interval_ms\":%d," + "\"battery_report\":%s}", + g_cfg.sleep_timeout_s, + g_cfg.display_brightness, + g_cfg.tag_name, + g_cfg.uwb_channel, + g_cfg.ranging_interval_ms, + g_cfg.battery_report ? "true" : "false"); +} + +/* ── Minimal JSON field extractor ──────────────────────────────── */ + +static const char *json_find_key(const char *json, const char *key) { + char needle[32]; + snprintf(needle, sizeof(needle), "\"%s\"", key); + const char *p = strstr(json, needle); + if (!p) return nullptr; + p += strlen(needle); + while (*p == ' ' || *p == ':') p++; + return p; +} + +static bool json_get_int(const char *json, const char *key, int *out) { + const char *p = json_find_key(json, key); + if (!p || *p == '\0') return false; + char *end; + long v = strtol(p, &end, 10); + if (end == p) return false; + *out = (int)v; + return true; +} + +static bool json_get_bool(const char *json, const char *key, bool *out) { + const char *p = json_find_key(json, key); + if (!p) return false; + if (strncmp(p, "true", 4) == 0) { *out = true; return true; } + if (strncmp(p, "false", 5) == 0) { *out = false; return true; } + return false; +} + +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++; + const char *end = strchr(p, '"'); + if (!end) return false; + size_t slen = end - p; + if (slen >= len) return false; + memcpy(buf, p, slen); + buf[slen] = '\0'; + return true; +} + +static const char *cfg_parse_json(const char *json) { + int v_int; + bool v_bool; + char v_str[17]; + + if (json_get_int(json, "sleep_timeout_s", &v_int)) { + if (v_int < 5 || v_int > 3600) return "sleep_timeout_s out of range [5..3600]"; + g_cfg.sleep_timeout_s = (uint16_t)v_int; + } + if (json_get_int(json, "display_brightness", &v_int)) { + if (v_int < 0 || v_int > 255) return "display_brightness out of range [0..255]"; + g_cfg.display_brightness = (uint8_t)v_int; + } + if (json_get_str(json, "tag_name", v_str, sizeof(v_str))) { + strncpy(g_cfg.tag_name, v_str, sizeof(g_cfg.tag_name) - 1); + g_cfg.tag_name[sizeof(g_cfg.tag_name) - 1] = '\0'; + } + if (json_get_int(json, "uwb_channel", &v_int)) { + if (v_int < 1 || v_int > 7) return "uwb_channel out of range [1..7]"; + g_cfg.uwb_channel = (uint8_t)v_int; + } + if (json_get_int(json, "ranging_interval_ms", &v_int)) { + if (v_int < 50 || v_int > 2000) return "ranging_interval_ms out of range [50..2000]"; + g_cfg.ranging_interval_ms = (uint16_t)v_int; + } + if (json_get_bool(json, "battery_report", &v_bool)) { + g_cfg.battery_report = v_bool; + } + return nullptr; +} + +/* ── BLE server callbacks ────────────────────────────────────────── */ + +class BleServerCb : public BLEServerCallbacks { + void onConnect(BLEServer *) override { + g_ble_connected = true; + Serial.println("[ble] Client connected"); + } + void onDisconnect(BLEServer *server) override { + g_ble_connected = false; + Serial.println("[ble] Client disconnected — restarting advertising"); + server->startAdvertising(); + } +}; + +class BleCfgCb : public BLECharacteristicCallbacks { + void onRead(BLECharacteristic *ch) override { + char buf[256]; + cfg_to_json(buf, sizeof(buf)); + ch->setValue(buf); + Serial.printf("[ble] Config read: %s\r\n", buf); + } + + 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()); + if (err) { + char msg[64]; + snprintf(msg, sizeof(msg), "+ERR:%s", err); + ble_notify_status(msg); + Serial.printf("[ble] Config error: %s\r\n", err); + return; + } + cfg_save(); + cfg_apply(); + ble_notify_status("+OK"); + Serial.println("[ble] Config applied OK"); + } + + static void ble_notify_status(const char *msg) { + if (g_ble_sts_char) { + g_ble_sts_char->setValue(msg); + g_ble_sts_char->notify(); + } + } +}; + +/* ── BLE init ──────────────────────────────────────────────────── */ + +static void ble_init(void) { + BLEDevice::init(g_ble_device_name); + BLEServer *server = BLEDevice::createServer(); + server->setCallbacks(new BleServerCb()); + + BLEService *svc = server->createService(BLE_SVC_UUID); + + g_ble_cfg_char = svc->createCharacteristic( + BLE_CFG_UUID, + BLECharacteristic::PROPERTY_READ | + BLECharacteristic::PROPERTY_WRITE | + BLECharacteristic::PROPERTY_WRITE_NR + ); + g_ble_cfg_char->setCallbacks(new BleCfgCb()); + + g_ble_sts_char = svc->createCharacteristic( + BLE_STS_UUID, + BLECharacteristic::PROPERTY_READ | + BLECharacteristic::PROPERTY_NOTIFY + ); + g_ble_sts_char->addDescriptor(new BLE2902()); + g_ble_sts_char->setValue("+OK"); + + svc->start(); + + BLEAdvertising *adv = BLEDevice::getAdvertising(); + adv->addServiceUUID(BLE_SVC_UUID); + adv->setScanResponse(true); + adv->setMinPreferred(0x06); + adv->setMinPreferred(0x12); + BLEDevice::startAdvertising(); + + Serial.printf("[ble] Advertising as \"%s\"\r\n", g_ble_device_name); + Serial.printf("[ble] Service UUID: %s\r\n", BLE_SVC_UUID); + Serial.printf("[ble] Config UUID: %s\r\n", BLE_CFG_UUID); + Serial.printf("[ble] Status UUID: %s\r\n", BLE_STS_UUID); +} + /* ── ESP-NOW ────────────────────────────────────────────────────── */ #define ESPNOW_MAGIC_0 0x5B @@ -245,38 +516,34 @@ struct EspNowPacket { uint8_t _pad; }; -/* Extended IMU packet — fits in 20 bytes too */ struct EspNowImuPacket { - uint8_t magic[2]; /* 0x5B 0x01 */ + uint8_t magic[2]; uint8_t tag_id; - uint8_t msg_type; /* MSG_IMU or MSG_FALL */ - int16_t ax_mg; /* accel X in milli-g */ - int16_t ay_mg; /* accel Y in milli-g */ - int16_t az_mg; /* accel Z in milli-g */ - int16_t gx_dps10; /* gyro X in 0.1 deg/s */ - int16_t gy_dps10; /* gyro Y */ - int16_t gz_dps10; /* gyro Z */ - uint8_t accel_mag_10; /* total accel in 0.1g (max 25.5g) */ + uint8_t msg_type; + int16_t ax_mg; + int16_t ay_mg; + int16_t az_mg; + int16_t gx_dps10; + int16_t gy_dps10; + int16_t gz_dps10; + uint8_t accel_mag_10; uint8_t seq_num; - uint16_t timestamp_s10; /* uptime in 0.1s (wraps at ~109 min) */ + uint16_t timestamp_s10; }; #pragma pack(pop) -/* ── Anchor tracking (declared early for ESP-NOW RX callback) ──── */ +/* ── Anchor tracking ────────────────────────────────────────────── */ static int32_t g_anchor_range_mm[NUM_ANCHORS]; static float g_anchor_rssi[NUM_ANCHORS]; static uint32_t g_anchor_last_ok[NUM_ANCHORS]; -/* ── ESP-NOW RX: receive range data from anchors ────────────────── */ - static void IRAM_ATTR espnow_rx_cb(const esp_now_recv_info_t *info, const uint8_t *data, int len) { if (len < (int)sizeof(EspNowPacket)) return; const EspNowPacket *pkt = (const EspNowPacket *)data; if (pkt->magic[0] != ESPNOW_MAGIC_0 || pkt->magic[1] != ESPNOW_MAGIC_1) return; - /* Only process anchor range broadcasts (anchor_id 0 or 1) */ if (pkt->msg_type == MSG_RANGE && pkt->anchor_id < NUM_ANCHORS) { int idx = pkt->anchor_id; g_anchor_range_mm[idx] = pkt->range_mm; @@ -285,8 +552,6 @@ static void IRAM_ATTR espnow_rx_cb(const esp_now_recv_info_t *info, } } -/* ── ESP-NOW TX ─────────────────────────────────────────────────── */ - static void espnow_send(uint8_t msg_type, uint8_t anchor_id, int32_t range_mm, float rssi) { EspNowPacket pkt = {}; @@ -298,7 +563,7 @@ 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 = 0xFF; /* TODO: battery ADC */ + pkt.battery_pct = g_cfg.battery_report ? 0xFE : 0xFF; pkt.flags = 0; pkt.seq_num = g_seq++; esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); @@ -322,7 +587,7 @@ static void espnow_send_imu(const ImuData *d, uint8_t msg_type) { esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); } -/* ── Anchor tracking (DW1000 address mapping) ──────────────────── */ +/* ── DW1000 address mapping ─────────────────────────────────────── */ static uint16_t g_anchor_addrs[NUM_ANCHORS] = {0, 0}; static int g_num_known_anchors = 0; @@ -343,17 +608,11 @@ static int anchor_index(uint16_t addr) { /* ── E-Stop ─────────────────────────────────────────────────────── */ static bool g_estop_active = false; -static uint32_t g_estop_last_tx = 0; static void estop_check(void) { - /* GPIO 0 (BOOT) floats LOW on Makerfabs display board — disabled. - * Fall detection acts as automatic e-stop instead. */ - - /* Auto e-stop on fall detection */ if (g_fall_detected && !g_estop_active) { g_estop_active = true; Serial.println("+ESTOP:FALL"); - /* Send immediate e-stop */ for (int i = 0; i < 3; i++) { EspNowPacket pkt = {}; pkt.magic[0] = ESPNOW_MAGIC_0; @@ -366,11 +625,9 @@ static void estop_check(void) { esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); delay(5); } - /* Also send fall event */ espnow_send_imu(&g_imu, MSG_FALL); } - /* Auto-clear after cooldown */ if (g_estop_active && !g_fall_detected) { g_estop_active = false; Serial.println("+ESTOP:CLEAR"); @@ -388,6 +645,7 @@ 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) @@ -396,32 +654,25 @@ static void estop_check(void) { * DEEP_SLEEP → ESP32 deep sleep (<0.5mA) * Triggered by GPIO0 held 3s * Wake by GPIO0 press (EXT0) - * - * Activity resets the idle timer and wakes display + DW1000. * ─────────────────────────────────────────────────────────────── */ -#define PM_DISPLAY_TIMEOUT_MS (30UL * 1000) /* 30s → display off */ #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 */ +#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; -/* DW1000 deep sleep state */ static bool g_dw1000_sleeping = false; -static uint32_t g_dw1000_sleep_ms = 0; /* when DW1000 entered sleep */ -static uint32_t g_dw1000_wake_ms = 0; /* when current wake started */ +static uint32_t g_dw1000_sleep_ms = 0; +static uint32_t g_dw1000_wake_ms = 0; static bool g_dw1000_in_window = false; -/* GPIO0 hold tracking */ static uint32_t g_btn_held_ms = 0; -/* Forward declaration */ -static void dw1000_ranging_init(void); +static void dw1000_ranging_init(void); /* forward declaration */ -/* Called on any ranging activity — resets idle timer, wakes hw */ static void pm_activity(void) { g_pm_last_activity_ms = millis(); @@ -440,7 +691,6 @@ static void pm_dw1000_sleep(void) { g_dw1000_in_window = false; } -/* Wake DW1000 and reinitialize ranging */ static void pm_dw1000_wake(void) { Serial.println("[pm] DW1000 waking"); DW1000.spiWakeup(); @@ -468,42 +718,39 @@ static void pm_enter_deep_sleep(void) { delay(800); display.ssd1306_command(SSD1306_DISPLAYOFF); - /* DW1000 deep sleep before ESP32 sleeps */ if (!g_dw1000_sleeping) { DW1000.deepSleep(); } - /* Wake on GPIO0 low (button press) */ 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; + uint32_t now = millis(); + uint32_t idle_ms = now - g_pm_last_activity_ms; - /* ── OLED auto-off ─────────────────────────────────────────── */ - if (g_pm_display_on && idle_ms >= PM_DISPLAY_TIMEOUT_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.println("[pm] Display off (30s idle)"); + Serial.printf("[pm] Display off (%ds idle)\r\n", g_cfg.sleep_timeout_s); } - /* ── DW1000 sleep ──────────────────────────────────────────── */ + /* DW1000 deep sleep */ if (!g_dw1000_sleeping) { if (idle_ms >= PM_DW1000_SLEEP_MS) { pm_dw1000_sleep(); } } else { - /* Periodic scan window */ - 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; + 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) { - /* End of scan window with no activity → back to sleep */ Serial.println("[pm] DW1000 scan window done — back to sleep"); pm_dw1000_sleep(); } else if (period_elapsed) { @@ -511,13 +758,12 @@ static void pm_update(void) { } } - /* ── GPIO0 hold → deep sleep ───────────────────────────────── */ + /* 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(); - /* unreachable */ } } else { g_btn_held_ms = 0; @@ -552,7 +798,7 @@ static void display_update(void) { return; } - /* Show sleep status when DW1000 is sleeping */ + /* 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); @@ -570,16 +816,22 @@ static void display_update(void) { return; } + /* BLE connected indicator */ + if (g_ble_connected) { + display.setTextSize(1); + display.setTextColor(SSD1306_WHITE); + display.setCursor(88, 0); + display.print(F("[BLE]")); + } + uint32_t now = millis(); - /* Find closest anchor */ int32_t min_range = INT32_MAX; for (int i = 0; i < NUM_ANCHORS; i++) { if (g_anchor_range_mm[i] > 0 && g_anchor_range_mm[i] < min_range) min_range = g_anchor_range_mm[i]; } - /* Line 1: Big distance */ display.setTextSize(3); display.setTextColor(SSD1306_WHITE); display.setCursor(0, 0); @@ -593,7 +845,6 @@ static void display_update(void) { display.println(F("---")); } - /* Line 2: Both anchor ranges */ display.setTextSize(1); display.setCursor(0, 28); for (int i = 0; i < NUM_ANCHORS; i++) { @@ -604,23 +855,16 @@ static void display_update(void) { } } - /* Line 3: IMU data or link status */ display.setCursor(0, 40); if (g_imu_ok) { - /* Show accel magnitude + tilt indicator */ display.printf("%.1fg", g_imu.accel_mag); - - /* Mini tilt bar (visual lean indicator) */ int tilt_x = constrain((int)(g_imu.ax * 20.0f), -30, 30); int bar_center = 80; display.drawFastHLine(bar_center - 30, 44, 60, SSD1306_WHITE); display.fillRect(bar_center + tilt_x - 2, 42, 4, 4, SSD1306_WHITE); - - /* Gyro Z (yaw rate) */ display.setCursor(100, 40); display.printf("%+.0f", g_imu.gz); } else { - /* No IMU — show link status */ bool any_linked = false; for (int i = 0; i < NUM_ANCHORS; i++) { if (g_anchor_last_ok[i] > 0 && (now - g_anchor_last_ok[i]) < 2000) { @@ -631,7 +875,6 @@ static void display_update(void) { display.println(any_linked ? F("LINKED") : F("SEARCHING...")); } - /* Line 4: Uptime + seq + IMU status */ display.setCursor(0, 54); uint32_t secs = now / 1000; display.printf("UP%02d:%02d p:%d", secs / 60, secs % 60, g_seq); @@ -670,12 +913,10 @@ static void newRange(void) { delay(1); digitalWrite(PIN_LED, LOW); - /* Range received → mark activity, wake display/DW1000 if needed */ + /* Activity: reset idle timer, wake display, exit DW1000 scan window */ pm_activity(); - - /* If DW1000 was in a scan window, it's awake — keep it up */ if (g_dw1000_in_window) { - g_dw1000_in_window = false; /* anchor found, stay fully active */ + g_dw1000_in_window = false; } } @@ -687,7 +928,7 @@ static void inactiveDevice(DW1000Device *device) { Serial.printf("+GONE:%04X\r\n", device->getShortAddress()); } -/* ── DW1000 init helper (used at startup and after wakeup) ─────── */ +/* ── DW1000 init helper (startup + wakeup) ──────────────────────── */ static void dw1000_ranging_init(void) { DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ); @@ -707,7 +948,6 @@ void setup(void) { pinMode(PIN_LED, OUTPUT); digitalWrite(PIN_LED, LOW); - /* Log wakeup cause after deep sleep */ 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"); @@ -715,6 +955,12 @@ void setup(void) { 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 */ + uint8_t mac[6]; + WiFi.macAddress(mac); + snprintf(g_ble_device_name, sizeof(g_ble_device_name), + "UWB_TAG_%02X%02X", mac[4], mac[5]); + /* I2C bus (shared: OLED @0x3C + MPU6050 @0x68) */ Wire.begin(PIN_SDA, PIN_SCL); @@ -731,25 +977,22 @@ void setup(void) { display.setCursor(0, 20); display.printf("Tag 0x%02X", TAG_ID); display.setCursor(0, 35); - display.println(F("DW1000 200m + ESP-NOW")); + display.println(F("DW1000 + BLE + PM")); display.setCursor(0, 50); - display.println(F("Init IMU...")); + display.printf("BLE: %s", g_ble_device_name); display.display(); } + /* Load NVS config (requires g_ble_device_name for default tag_name) */ + cfg_load(); + 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)"); - display.setCursor(0, 50); - display.println(F("IMU OK! Searching...")); - display.display(); - } else { + else Serial.println("[uwb_tag] MPU6050 not found — running without IMU"); - display.setCursor(0, 50); - display.println(F("No IMU. Searching...")); - display.display(); - } /* ESP-NOW */ WiFi.mode(WIFI_STA); @@ -767,11 +1010,14 @@ void setup(void) { Serial.println("[uwb_tag] WARN: ESP-NOW failed"); } + /* BLE */ + ble_init(); + /* DW1000 */ SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI); dw1000_ranging_init(); - /* Init state */ + /* Init anchor state */ for (int i = 0; i < NUM_ANCHORS; i++) { g_anchor_range_mm[i] = -1; g_anchor_rssi[i] = -100.0f; @@ -781,9 +1027,9 @@ void setup(void) { g_pm_last_activity_ms = millis(); Serial.println("[uwb_tag] DW1000 ready MODE_LONGDATA_RANGE_LOWPOWER"); - Serial.println("[uwb_tag] Ranging + display + ESP-NOW active"); + 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", - PM_DISPLAY_TIMEOUT_MS / 1000, + g_cfg.sleep_timeout_s, PM_DW1000_SLEEP_MS / 1000, PM_DEEP_SLEEP_HOLD_MS / 1000); } @@ -794,29 +1040,27 @@ static uint32_t g_last_hb = 0; static uint32_t g_last_imu = 0; void loop(void) { - /* IMU read at ~50 Hz (fast enough for fall detection) */ + /* IMU read at ~50 Hz */ if (g_imu_ok && (millis() - g_last_imu >= 20)) { mpu6050_read(&g_imu); fall_detect(&g_imu); g_last_imu = millis(); } - /* E-stop (now triggered by fall detection) */ estop_check(); - /* Heartbeat every 1s — includes IMU data if available */ + /* Heartbeat every 1s */ if (millis() - g_last_hb >= 1000) { espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f); g_last_hb = millis(); } - /* IMU broadcast at 10 Hz via ESP-NOW */ + /* IMU broadcast at 10 Hz */ static uint32_t s_last_imu_tx = 0; if (g_imu_ok && (millis() - s_last_imu_tx >= 100)) { espnow_send_imu(&g_imu, MSG_IMU); s_last_imu_tx = millis(); - /* Serial output for debugging / ROS */ Serial.printf("+IMU:%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.2f\r\n", g_imu.ax, g_imu.ay, g_imu.az, g_imu.gx, g_imu.gy, g_imu.gz, @@ -824,9 +1068,12 @@ void loop(void) { s_last_imu_tx = millis(); } - /* DW1000 ranging — skip while DW1000 is in deep sleep */ - if (!g_dw1000_sleeping && !g_estop_active) { + /* DW1000 ranging — skip while sleeping, throttle by ranging_interval_ms */ + static uint32_t s_last_range_tick = 0; + if (!g_dw1000_sleeping && !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) */