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 d83bbac..0f5d991 100644 --- a/esp32/uwb_tag/src/main.cpp +++ b/esp32/uwb_tag/src/main.cpp @@ -27,6 +27,25 @@ * I2C: SDA=4 SCL=5 OLED @0x3C, MPU6050 @0x68 * E-Stop: GPIO 0 (BOOT), active LOW — disabled (floats LOW) * LED: GPIO 2 + * + * 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 @@ -35,6 +54,11 @@ #include #include #include +#include +#include +#include +#include +#include #include "DW1000Ranging.h" #include @@ -78,25 +102,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; @@ -119,24 +142,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; } @@ -150,40 +165,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) { @@ -196,7 +197,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; @@ -204,6 +204,299 @@ 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: + * {"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:" on fail. + * 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]; /* max 16 chars + NUL */ + 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); /* 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); + 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(); + + /* 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; + + 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); /* 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); + 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"); +} + +/* 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," + "\"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 ──────────────────────────────── + * 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); + 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; +} + +/* 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++; /* skip opening quote */ + 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; +} + +/* 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; + 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; /* success */ +} + +/* ── 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 { + /* READ: return current config as JSON */ + 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); + } + + /* 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()); // String::c_str() is valid + 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); + + /* Config characteristic — readable and writable */ + 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()); + + /* Status characteristic — readable and notifiable */ + 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 @@ -232,38 +525,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; @@ -272,8 +561,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 = {}; @@ -285,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 = 0xFF; /* TODO: battery ADC */ + /* 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)); @@ -309,7 +597,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; @@ -330,17 +618,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; @@ -353,11 +635,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"); @@ -400,16 +680,22 @@ static void display_update(void) { return; } + /* Show BLE info briefly when connected */ + 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); @@ -423,7 +709,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++) { @@ -434,23 +719,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) { @@ -461,7 +739,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); @@ -521,6 +798,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); @@ -537,24 +820,24 @@ 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 200m + BLE cfg")); display.setCursor(0, 50); - display.println(F("Init IMU...")); + display.printf("BLE: %s", g_ble_device_name); display.display(); } + /* 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) { Serial.println("[uwb_tag] MPU6050 init OK (±8g, ±500dps)"); - display.setCursor(0, 50); - display.println(F("IMU OK! Searching...")); - display.display(); } 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 */ @@ -573,17 +856,18 @@ void setup(void) { Serial.println("[uwb_tag] WARN: ESP-NOW failed"); } + /* BLE */ + ble_init(); + /* DW1000 */ SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI); 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 state */ + /* Init anchor state */ for (int i = 0; i < NUM_ANCHORS; i++) { g_anchor_range_mm[i] = -1; g_anchor_rssi[i] = -100.0f; @@ -591,7 +875,7 @@ void setup(void) { } 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 active"); } /* ── Loop ───────────────────────────────────────────────────────── */ @@ -600,29 +884,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, @@ -630,9 +912,12 @@ void loop(void) { s_last_imu_tx = millis(); } - /* DW1000 ranging */ - if (!g_estop_active) { + /* DW1000 ranging — throttled by configured interval */ + static uint32_t s_last_range_tick = 0; + if (!g_estop_active && + (millis() - s_last_range_tick >= g_cfg.ranging_interval_ms)) { DW1000Ranging.loop(); + s_last_range_tick = millis(); } /* Display at 5 Hz */