diff --git a/esp32/uwb_tag/platformio.ini b/esp32/uwb_tag/platformio.ini index 2c9f3ad..2520868 100644 --- a/esp32/uwb_tag/platformio.ini +++ b/esp32/uwb_tag/platformio.ini @@ -1,8 +1,17 @@ -; SaltyBot UWB Tag Firmware -; Target: Makerfabs ESP32 UWB Pro with Display (DW1000 + SSD1306 OLED) +; SaltyBot UWB Tag Firmware — Issue #545, Issue #698 +; Target: Makerfabs ESP32 UWB Pro with Display (DW3000 + SSD1306 OLED) +; +; The tag is battery-powered, worn by the person being tracked. +; It initiates DS-TWR ranging with each anchor in round-robin, +; shows status on OLED display, and sends data via ESP-NOW. +; +; Library: Makerfabs MaUWB_DW3000 +; https://github.com/Makerfabs/MaUWB_DW3000 ; ; Flash: ; pio run -e tag --target upload +; Monitor (USB debug): +; pio device monitor -b 115200 [env:tag] platform = espressif32 @@ -10,12 +19,11 @@ board = esp32dev framework = arduino monitor_speed = 115200 upload_speed = 921600 -lib_extra_dirs = ../../lib lib_deps = + https://github.com/Makerfabs/MaUWB_DW3000.git 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 - -Wno-error=return-type + -DTAG_ID=0x01 ; unique per tag (0x01–0xFE) + -DRANGE_INTERVAL_MS=50 ; 20 Hz round-robin across discovered anchors diff --git a/esp32/uwb_tag/src/main.cpp b/esp32/uwb_tag/src/main.cpp index 6ff653c..1b8e10f 100644 --- a/esp32/uwb_tag/src/main.cpp +++ b/esp32/uwb_tag/src/main.cpp @@ -1,99 +1,99 @@ /* - * uwb_tag — SaltyBot ESP32 UWB Pro with Display (DW1000 TWR initiator) + * uwb_tag — SaltyBot ESP32 UWB Pro tag firmware (DS-TWR initiator) + * Issue #545 + display/ESP-NOW/e-stop extensions + * Issue #698: anchor auto-discovery * - * Hardware: Makerfabs ESP32 UWB Pro with Display - * DW1000 (200m range), SSD1306 128x64 OLED, LiPo battery - * MPU6050 (accel+gyro) on shared I2C bus @0x68 + * Hardware: Makerfabs ESP32 UWB Pro with Display (DW3000 + SSD1306 OLED) * - * Worn by person on EUC while robot follows. - * Ranges with 2 anchors on robot, shows distance on OLED, - * broadcasts data via ESP-NOW, has e-stop on GPIO 0. + * Role + * ──── + * Tag is worn by a person riding an EUC while SaltyBot follows. + * Initiates DS-TWR ranging with discovered anchors at ~20 Hz round-robin. + * Shows distance/status on OLED. Sends range data via ESP-NOW. + * GPIO 0 = emergency stop button (active low). * - * OLED display (5 Hz): - * Line 1: Big distance to nearest anchor - * Line 2: Per-anchor ranges - * Line 3: Link status + RSSI bars (or IMU accel) - * Line 4: Uptime + packet count + * Anchor auto-discovery (Issue #698) + * ──────────────────────────────────── + * No anchor firmware changes required. Anchors respond only to POLLs + * addressed to their own ID, so the tag probes IDs 0..DISC_PROBE_IDS-1 + * using the standard POLL frame — any that answer are "discovered". * - * ESP-NOW broadcast (20-byte packets): - * Range data after each successful TWR cycle - * Heartbeat every 1s even if ranging fails - * E-stop at 10 Hz while button held - * IMU data at 10 Hz (piggybacked on MSG_IMU) + * State machine + * ───────────── + * DISC_SCAN (startup + periodic re-scan every DISC_RESCAN_MS) + * Probes each unconfirmed ID once per ranging slot. + * IDs that respond enter the active anchor table. + * DISC_RANGE (normal operation) + * Round-robin ranging across active anchors only. + * An anchor that misses ANCHOR_MISS_LIMIT consecutive cycles, or goes + * silent for ANCHOR_TIMEOUT_MS, is removed from the table. + * Re-scan triggered when table empties or DISC_RESCAN_MS elapses. * - * Pin map — ESP32 UWB Pro with Display: - * SPI: SCK=18 MISO=19 MOSI=23 CS=21 (not 4! display board differs) - * DW1000: RST=27 IRQ=34 - * I2C: SDA=4 SCL=5 OLED @0x3C, MPU6050 @0x68 - * E-Stop: GPIO 0 (BOOT), active LOW — disabled (floats LOW) - * LED: GPIO 2 + * Serial output (USB, 115200) + * ──────────────────────────── + * +RANGE:,,\r\n + * +DISC:FOUND,\r\n — anchor added to table + * +DISC:LOST,\r\n — anchor removed from table * - * Power management (Issue #689): - * OLED auto-off after sleep_timeout_s inactivity → saves ~25mA - * DW1000 deep sleep after 5min idle → saves ~155mA (160mA→3.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) + * ESP-NOW packet (broadcast, 20 bytes) — layout unchanged + * ───────────────────────────────────────────────────────── + * [0-1] magic 0x5B 0x01 + * [2] tag_id + * [3] msg_type 0x10=range 0x20=estop 0x30=heartbeat 0x40=disc + * [4] anchor_id + * [5-8] range_mm (int32_t LE) — -1 for DISC:LOST + * [9-12] rssi_dbm (float LE) + * [13-16] timestamp (uint32_t millis) + * [17] battery_pct (0-100 or 0xFF) + * [18] flags bit0=estop_active bit1=disc_scan_active + * [19] seq_num_lo (uint8_t, rolling) * - * 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 + * Pin mapping — Makerfabs ESP32 UWB Pro with Display + * ────────────────────────────────────────────────── + * SPI SCK 18 SPI MISO 19 SPI MOSI 23 + * DW CS 21 DW RST 27 DW IRQ 34 + * I2C SDA 4 I2C SCL 5 OLED addr 0x3C + * LED 2 E-STOP 0 (BOOT, active LOW) */ #include #include #include +#include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include "DW1000Ranging.h" + +#include "dw3000.h" #include #include -/* ── Config ─────────────────────────────────────────────────────── */ +/* ── Configurable ───────────────────────────────────────────────── */ #ifndef TAG_ID -# define TAG_ID 0x01 +# define TAG_ID 0x01 #endif -static char TAG_ADDR[] = "7D:00:22:EA:82:60:3B:9B"; +#ifndef RANGE_INTERVAL_MS +# define RANGE_INTERVAL_MS 50 /* 20 Hz round-robin */ +#endif -#define SERIAL_BAUD 115200 -#define NUM_ANCHORS 2 +#define SERIAL_BAUD 115200 + +/* ── Discovery parameters ───────────────────────────────────────── */ + +#define DISC_PROBE_IDS 8 /* scan anchor IDs 0x00..0x07 */ +#define DISC_RESCAN_MS 10000UL /* periodic re-scan interval */ +#define ANCHOR_MISS_LIMIT 5 /* consecutive misses → stale */ +#define ANCHOR_TIMEOUT_MS 5000UL /* silence before removal */ /* ── Pins ───────────────────────────────────────────────────────── */ #define PIN_SCK 18 #define PIN_MISO 19 #define PIN_MOSI 23 -#define PIN_CS 21 /* Display board uses 21, not 4 */ +#define PIN_CS 21 #define PIN_RST 27 #define PIN_IRQ 34 @@ -101,402 +101,23 @@ static char TAG_ADDR[] = "7D:00:22:EA:82:60:3B:9B"; #define PIN_SCL 5 #define PIN_LED 2 -#define PIN_ESTOP 0 +#define PIN_ESTOP 0 /* BOOT button, active LOW */ /* ── OLED ───────────────────────────────────────────────────────── */ -Adafruit_SSD1306 display(128, 64, &Wire, -1); - -/* ── MPU6050 (I2C @0x68, shared bus with OLED) ──────────────────── */ - -#define MPU6050_ADDR 0x68 -#define MPU6050_WHO_AM_I 0x75 -#define MPU6050_PWR_MGMT_1 0x6B -#define MPU6050_ACCEL_CFG 0x1C -#define MPU6050_GYRO_CFG 0x1B -#define MPU6050_ACCEL_XOUT 0x3B -#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; - float gx, gy, gz; - float accel_mag; -}; - -static ImuData g_imu = {}; - -#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; -static bool g_freefall_phase = false; -static uint32_t g_freefall_start = 0; - -static void mpu6050_write(uint8_t reg, uint8_t val) { - Wire.beginTransmission(MPU6050_ADDR); - Wire.write(reg); - Wire.write(val); - Wire.endTransmission(); -} - -static uint8_t mpu6050_read_byte(uint8_t reg) { - Wire.beginTransmission(MPU6050_ADDR); - Wire.write(reg); - Wire.endTransmission(false); - Wire.requestFrom((uint8_t)MPU6050_ADDR, (uint8_t)1); - return Wire.read(); -} - -static bool mpu6050_init(void) { - 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); - mpu6050_write(MPU6050_PWR_MGMT_1, 0x01); - delay(10); - mpu6050_write(MPU6050_ACCEL_CFG, 0x10); - mpu6050_write(MPU6050_GYRO_CFG, 0x08); - return true; -} - -static void mpu6050_read(ImuData *d) { - Wire.beginTransmission(MPU6050_ADDR); - Wire.write(MPU6050_ACCEL_XOUT); - Wire.endTransmission(false); - Wire.requestFrom((uint8_t)MPU6050_ADDR, (uint8_t)14); - - int16_t raw[7]; - for (int i = 0; i < 7; i++) { - raw[i] = (Wire.read() << 8) | Wire.read(); - } - d->ax = raw[0] / 4096.0f; - d->ay = raw[1] / 4096.0f; - d->az = raw[2] / 4096.0f; - 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); -} - -static void fall_detect(const ImuData *d) { - uint32_t now = millis(); - if (g_fall_detected && (now - g_fall_last_ms) < FALL_COOLDOWN_MS) return; - g_fall_detected = false; - - if (d->accel_mag < FALL_FREEFALL_LOW) { - if (!g_freefall_phase) { - g_freefall_phase = true; - g_freefall_start = now; - } - } else { - if (g_freefall_phase && (now - g_freefall_start) > 50) { - if (d->accel_mag > FALL_ACCEL_THRESHOLD && - (now - g_freefall_start) < 500) { - g_fall_detected = true; - g_fall_last_ms = now; - Serial.printf("+FALL:%.1fg freefall=%dms\r\n", - d->accel_mag, now - g_freefall_start); - } - } - g_freefall_phase = false; - } - - if (!g_fall_detected && d->accel_mag > 3.5f) { - g_fall_detected = true; - g_fall_last_ms = now; - Serial.printf("+FALL:%.1fg impact\r\n", d->accel_mag); - } -} - -/* ── 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); -} +#define SCREEN_W 128 +#define SCREEN_H 64 +Adafruit_SSD1306 display(SCREEN_W, SCREEN_H, &Wire, -1); /* ── ESP-NOW ────────────────────────────────────────────────────── */ -#define ESPNOW_MAGIC_0 0x5B -#define ESPNOW_MAGIC_1 0x01 -#define MSG_RANGE 0x10 -#define MSG_ESTOP 0x20 -#define MSG_HEARTBEAT 0x30 -#define MSG_IMU 0x40 -#define MSG_FALL 0x50 +#define ESPNOW_MAGIC_0 0x5B +#define ESPNOW_MAGIC_1 0x01 + +#define MSG_RANGE 0x10 +#define MSG_ESTOP 0x20 +#define MSG_HEARTBEAT 0x30 +#define MSG_DISC 0x40 /* anchor table changed */ static uint8_t broadcast_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; static uint8_t g_seq = 0; @@ -515,43 +136,136 @@ struct EspNowPacket { uint8_t seq_num; uint8_t _pad; }; - -struct EspNowImuPacket { - uint8_t magic[2]; - uint8_t tag_id; - 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; -}; #pragma pack(pop) -/* ── Anchor tracking ────────────────────────────────────────────── */ +static_assert(sizeof(EspNowPacket) == 20, "packet must be 20 bytes"); -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]; +/* ── Anchor table ───────────────────────────────────────────────── */ -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; +struct AnchorEntry { + bool present; /* ever responded to a POLL */ + bool active; /* currently reachable */ + int32_t range_mm; /* last successful range */ + float rssi; /* last RSSI */ + uint32_t last_ok_ms; /* millis() of last good range */ + uint8_t miss_count; /* consecutive misses */ +}; - if (pkt->msg_type == MSG_RANGE && pkt->anchor_id < NUM_ANCHORS) { - int idx = pkt->anchor_id; - g_anchor_range_mm[idx] = pkt->range_mm; - g_anchor_rssi[idx] = pkt->rssi_dbm; - g_anchor_last_ok[idx] = millis(); - } +static AnchorEntry g_anchors[DISC_PROBE_IDS] = {}; + +static int anchor_count_active(void) { + int n = 0; + for (int i = 0; i < DISC_PROBE_IDS; i++) + if (g_anchors[i].active) n++; + return n; } +/* ── Discovery state ────────────────────────────────────────────── */ + +enum DiscState { DISC_SCAN, DISC_RANGE }; +static DiscState g_disc_state = DISC_SCAN; +static uint8_t g_disc_cursor = 0; +static uint32_t g_last_rescan = 0; +static bool g_disc_active = false; /* true while scanning */ + +/* ── E-Stop (also read by espnow_send flags byte) ───────────────── */ + +static bool g_estop_active = false; + +/* ── DW3000 PHY config (must match anchor) ──────────────────────── */ + +static dwt_config_t dw_cfg = { + 5, /* channel 5 */ + DWT_PLEN_128, + DWT_PAC8, + 9, 9, /* TX/RX preamble code */ + 1, /* SFD type */ + DWT_BR_6M8, + DWT_PHR_MODE_STD, + DWT_PHR_RATE_DATA, + (129 + 8 - 8), + DWT_STS_MODE_OFF, + DWT_STS_LEN_64, + DWT_PDOA_M0, +}; + +/* ── Frame format ──────────────────────────────────────────────── */ + +#define FTYPE_POLL 0x01 +#define FTYPE_RESP 0x02 +#define FTYPE_FINAL 0x03 + +#define FRAME_HDR 3 +#define FCS_LEN 2 + +#define POLL_FRAME_LEN (FRAME_HDR + FCS_LEN) +#define RESP_PAYLOAD 10 +#define RESP_FRAME_LEN (FRAME_HDR + RESP_PAYLOAD + FCS_LEN) +#define FINAL_PAYLOAD 15 +#define FINAL_FRAME_LEN (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN) + +/* ── Timing ────────────────────────────────────────────────────── */ + +#define FINAL_TX_DLY_US 500UL +#define DWT_TICKS_PER_US 63898UL +#define FINAL_TX_DLY_TICKS (FINAL_TX_DLY_US * DWT_TICKS_PER_US) +#define RESP_RX_TIMEOUT_US 3000 + +#define SPEED_OF_LIGHT 299702547.0 +#define DWT_TS_MASK 0xFFFFFFFFFFULL +#define ANT_DELAY 16385 + +/* ── ISR state ──────────────────────────────────────────────────── */ + +static volatile bool g_rx_ok = false; +static volatile bool g_tx_done = false; +static volatile bool g_rx_err = false; +static volatile bool g_rx_to = false; + +static uint8_t g_rx_buf[128]; +static uint32_t g_rx_len = 0; + +static void cb_tx_done(const dwt_cb_data_t *) { g_tx_done = true; } +static void cb_rx_ok(const dwt_cb_data_t *d) { + g_rx_len = d->datalength; + if (g_rx_len > sizeof(g_rx_buf)) g_rx_len = sizeof(g_rx_buf); + dwt_readrxdata(g_rx_buf, g_rx_len, 0); + g_rx_ok = true; +} +static void cb_rx_err(const dwt_cb_data_t *) { g_rx_err = true; } +static void cb_rx_to(const dwt_cb_data_t *) { g_rx_to = true; } + +/* ── Timestamp helpers ──────────────────────────────────────────── */ + +static uint64_t ts_read(const uint8_t *p) { + uint64_t v = 0; + for (int i = 4; i >= 0; i--) v = (v << 8) | p[i]; + return v; +} + +static void ts_write(uint8_t *p, uint64_t v) { + for (int i = 0; i < 5; i++, v >>= 8) p[i] = (uint8_t)(v & 0xFF); +} + +static inline uint64_t ts_diff(uint64_t later, uint64_t earlier) { + return (later - earlier) & DWT_TS_MASK; +} + +static inline double ticks_to_s(uint64_t t) { + return (double)t / (128.0 * 499200000.0); +} + +static float rx_power_dbm(void) { + dwt_rxdiag_t d; + dwt_readdiagnostics(&d); + if (d.maxGrowthCIR == 0 || d.rxPreamCount == 0) return 0.0f; + float f = (float)d.maxGrowthCIR; + float n = (float)d.rxPreamCount; + return 10.0f * log10f((f * f) / (n * n)) - 121.74f; +} + +/* ── ESP-NOW send ───────────────────────────────────────────────── */ + static void espnow_send(uint8_t msg_type, uint8_t anchor_id, int32_t range_mm, float rssi) { EspNowPacket pkt = {}; @@ -563,381 +277,333 @@ 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; - pkt.flags = 0; + pkt.battery_pct = 0xFF; + pkt.flags = (g_estop_active ? 0x01u : 0x00u) | + (g_disc_active ? 0x02u : 0x00u); pkt.seq_num = g_seq++; esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); } -static void espnow_send_imu(const ImuData *d, uint8_t msg_type) { - EspNowImuPacket pkt = {}; - pkt.magic[0] = ESPNOW_MAGIC_0; - pkt.magic[1] = ESPNOW_MAGIC_1; - pkt.tag_id = TAG_ID; - pkt.msg_type = msg_type; - pkt.ax_mg = (int16_t)(d->ax * 1000.0f); - pkt.ay_mg = (int16_t)(d->ay * 1000.0f); - pkt.az_mg = (int16_t)(d->az * 1000.0f); - pkt.gx_dps10 = (int16_t)(d->gx * 10.0f); - pkt.gy_dps10 = (int16_t)(d->gy * 10.0f); - pkt.gz_dps10 = (int16_t)(d->gz * 10.0f); - pkt.accel_mag_10 = (uint8_t)constrain((int)(d->accel_mag * 10.0f), 0, 255); - pkt.seq_num = g_seq++; - pkt.timestamp_s10 = (uint16_t)((millis() / 100) & 0xFFFF); - esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); -} - -/* ── DW1000 address mapping ─────────────────────────────────────── */ - -static uint16_t g_anchor_addrs[NUM_ANCHORS] = {0, 0}; -static int g_num_known_anchors = 0; - -static int anchor_index(uint16_t addr) { - for (int i = 0; i < g_num_known_anchors; i++) { - if (g_anchor_addrs[i] == addr) return i; - } - if (g_num_known_anchors < NUM_ANCHORS) { - int idx = g_num_known_anchors++; - g_anchor_addrs[idx] = addr; - Serial.printf("[uwb_tag] Mapped anchor %04X → A%d\r\n", addr, idx); - return idx; - } - return -1; -} - /* ── E-Stop ─────────────────────────────────────────────────────── */ -static bool g_estop_active = false; +static uint32_t g_estop_last_tx = 0; static void estop_check(void) { - if (g_fall_detected && !g_estop_active) { + bool pressed = (digitalRead(PIN_ESTOP) == LOW); + + if (pressed && !g_estop_active) { g_estop_active = true; - Serial.println("+ESTOP:FALL"); - for (int i = 0; i < 3; i++) { - EspNowPacket pkt = {}; - pkt.magic[0] = ESPNOW_MAGIC_0; - pkt.magic[1] = ESPNOW_MAGIC_1; - pkt.tag_id = TAG_ID; - pkt.msg_type = MSG_ESTOP; - pkt.flags = 0x01; - pkt.seq_num = g_seq++; - pkt.timestamp_ms = millis(); - esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); - delay(5); - } - espnow_send_imu(&g_imu, MSG_FALL); + Serial.println("+ESTOP:ACTIVE"); } - if (g_estop_active && !g_fall_detected) { + if (g_estop_active && pressed) { + if (millis() - g_estop_last_tx >= 100) { + espnow_send(MSG_ESTOP, 0xFF, 0, 0.0f); + g_estop_last_tx = millis(); + } + } + + if (!pressed && g_estop_active) { + for (int i = 0; i < 3; i++) { + g_estop_active = false; + espnow_send(MSG_ESTOP, 0xFF, 0, 0.0f); + delay(10); + } g_estop_active = false; Serial.println("+ESTOP:CLEAR"); - EspNowPacket pkt = {}; - pkt.magic[0] = ESPNOW_MAGIC_0; - pkt.magic[1] = ESPNOW_MAGIC_1; - pkt.tag_id = TAG_ID; - pkt.msg_type = MSG_ESTOP; - pkt.flags = 0x00; - pkt.seq_num = g_seq++; - pkt.timestamp_ms = millis(); - esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); } } -/* ── 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) - * ─────────────────────────────────────────────────────────────── */ +/* ── DS-TWR: one complete ranging cycle with one anchor ─────────── */ -#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 int32_t twr_range_once(uint8_t anchor_id) { -static uint32_t g_pm_last_activity_ms = 0; -static bool g_pm_display_on = true; + /* 1. TX POLL */ + uint8_t poll[POLL_FRAME_LEN]; + poll[0] = FTYPE_POLL; + poll[1] = TAG_ID; + poll[2] = anchor_id; -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; + dwt_writetxdata(POLL_FRAME_LEN - FCS_LEN, poll, 0); + dwt_writetxfctrl(POLL_FRAME_LEN, 0, 1); + dwt_setrxaftertxdelay(300); + dwt_setrxtimeout(RESP_RX_TIMEOUT_US); -static uint32_t g_btn_held_ms = 0; + g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false; + if (dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) + return -1; -static void dw1000_ranging_init(void); /* forward declaration */ + uint32_t t0 = millis(); + while (!g_tx_done && millis() - t0 < 15) yield(); -static void pm_activity(void) { - g_pm_last_activity_ms = millis(); + uint8_t poll_tx_raw[5]; + dwt_readtxtimestamp(poll_tx_raw); + uint64_t T_poll_tx = ts_read(poll_tx_raw); - if (!g_pm_display_on) { - display.ssd1306_command(SSD1306_DISPLAYON); - g_pm_display_on = true; - Serial.println("[pm] Display on (activity)"); + /* 2. Wait for RESP */ + t0 = millis(); + while (!g_rx_ok && !g_rx_err && !g_rx_to && millis() - t0 < 60) yield(); + if (!g_rx_ok || g_rx_len < FRAME_HDR + RESP_PAYLOAD) return -1; + if (g_rx_buf[0] != FTYPE_RESP) return -1; + if (g_rx_buf[2] != TAG_ID) return -1; + if (g_rx_buf[1] != anchor_id) return -1; + + uint8_t resp_rx_raw[5]; + dwt_readrxtimestamp(resp_rx_raw); + uint64_t T_resp_rx = ts_read(resp_rx_raw); + uint64_t T_poll_rx_a = ts_read(&g_rx_buf[3]); + uint64_t T_resp_tx_a = ts_read(&g_rx_buf[8]); + + /* 3. DS-TWR intermediate values */ + uint64_t Ra = ts_diff(T_resp_rx, T_poll_tx); + uint64_t Db = ts_diff(T_resp_tx_a, T_poll_rx_a); + + uint64_t final_tx_sched = (T_resp_rx + FINAL_TX_DLY_TICKS) & ~0x1FFULL; + uint64_t Da = ts_diff(final_tx_sched, T_resp_rx); + + /* 4. TX FINAL */ + uint8_t final_buf[FINAL_FRAME_LEN]; + final_buf[0] = FTYPE_FINAL; + final_buf[1] = TAG_ID; + final_buf[2] = anchor_id; + ts_write(&final_buf[3], Ra); + ts_write(&final_buf[8], Da); + ts_write(&final_buf[13], Db); + + dwt_writetxdata(FINAL_FRAME_LEN - FCS_LEN, final_buf, 0); + dwt_writetxfctrl(FINAL_FRAME_LEN, 0, 1); + dwt_setdelayedtrxtime((uint32_t)(final_tx_sched >> 8)); + + g_tx_done = false; + if (dwt_starttx(DWT_START_TX_DELAYED) != DWT_SUCCESS) { + dwt_forcetrxoff(); + return -1; } + t0 = millis(); + while (!g_tx_done && millis() - t0 < 15) yield(); + + /* 5. Tag-side range estimate */ + uint8_t final_tx_raw[5]; + dwt_readtxtimestamp(final_tx_raw); + /* T_final_tx unused on tag side — anchor computes authoritative range */ + + double ra = ticks_to_s(Ra); + double db = ticks_to_s(Db); + double tof = (ra - db) / 2.0; + double range_m = tof * SPEED_OF_LIGHT; + + if (range_m < 0.1 || range_m > 130.0) return -1; + return (int32_t)(range_m * 1000.0 + 0.5); } -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; -} +/* ── Anchor table management ────────────────────────────────────── */ -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(); -} +/* Range with anchor_id, update table, broadcast result. + * Returns range_mm on success, -1 on miss. */ +static int32_t range_and_update(uint8_t id) { + int32_t range_mm = twr_range_once(id); + AnchorEntry &a = g_anchors[id]; -static void pm_enter_deep_sleep(void) { - Serial.println("[pm] ESP32 deep sleep — GPIO0 to wake"); - Serial.flush(); + if (range_mm > 0) { + float rssi = rx_power_dbm(); + bool was_active = a.active; - 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); + a.present = true; + a.active = true; + a.range_mm = range_mm; + a.rssi = rssi; + a.last_ok_ms = millis(); + a.miss_count = 0; - if (!g_dw1000_sleeping) { - DW1000.deepSleep(); - } + Serial.printf("+RANGE:%d,%ld,%.1f\r\n", id, (long)range_mm, rssi); + espnow_send(MSG_RANGE, id, range_mm, rssi); - esp_sleep_enable_ext0_wakeup(GPIO_NUM_0, 0); - esp_deep_sleep_start(); - /* never returns */ -} + digitalWrite(PIN_LED, HIGH); + delay(2); + digitalWrite(PIN_LED, LOW); -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(); + if (!was_active) { + Serial.printf("+DISC:FOUND,%d\r\n", id); + espnow_send(MSG_DISC, id, range_mm, rssi); } - } 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; + } else if (a.present) { + a.miss_count++; + bool timed_out = (millis() - a.last_ok_ms) >= ANCHOR_TIMEOUT_MS; + bool hit_limit = (a.miss_count >= ANCHOR_MISS_LIMIT); - if (window_expired) { - Serial.println("[pm] DW1000 scan window done — back to sleep"); - pm_dw1000_sleep(); - } else if (period_elapsed) { - pm_dw1000_wake(); + if (a.active && (timed_out || hit_limit)) { + a.active = false; + Serial.printf("+DISC:LOST,%d misses=%d\r\n", id, a.miss_count); + espnow_send(MSG_DISC, id, -1, 0.0f); } } - - /* 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 { - if (g_btn_held_ms > 0 && (now - g_btn_held_ms) < 1000) { g_pm_last_activity_ms = now; display.ssd1306_command(SSD1306_DISPLAYON); } g_btn_held_ms = 0; - } + return range_mm; } -/* ── OLED display (5 Hz) ────────────────────────────────────────── */ +/* ── Discovery scan ─────────────────────────────────────────────── */ -static uint32_t g_disp_last = 0; +static void disc_start_scan(void) { + g_disc_state = DISC_SCAN; + g_disc_cursor = 0; + g_disc_active = true; + Serial.printf("[disc] Scanning IDs 0..%d\r\n", DISC_PROBE_IDS - 1); +} + +/* Advance scan by one probe per ranging slot. + * Returns true when the full scan is complete. */ +static bool disc_scan_step(void) { + /* Skip IDs already active — no need to probe them */ + while (g_disc_cursor < DISC_PROBE_IDS && + g_anchors[g_disc_cursor].active) { + g_disc_cursor++; + } + + if (g_disc_cursor >= DISC_PROBE_IDS) { + g_disc_state = DISC_RANGE; + g_disc_active = false; + g_last_rescan = millis(); + Serial.printf("[disc] Scan complete — %d active anchor(s)\r\n", + anchor_count_active()); + return true; + } + + range_and_update(g_disc_cursor++); + return false; +} + +/* ── OLED display update (5 Hz) ─────────────────────────────────── */ + +static uint32_t g_display_last = 0; static void display_update(void) { - if (!g_pm_display_on) return; - if (millis() - g_disp_last < 200) return; - g_disp_last = millis(); + if (millis() - g_display_last < 200) return; + g_display_last = millis(); display.clearDisplay(); + /* E-Stop takes full screen */ if (g_estop_active) { - display.setTextSize(2); + display.setTextSize(3); display.setTextColor(SSD1306_WHITE); - display.setCursor(10, 0); - display.println(F("!! FALL !!")); + display.setCursor(10, 4); + display.println(F("E-STOP")); display.setTextSize(1); - display.setCursor(0, 24); - display.printf("Impact: %.1fg", g_imu.accel_mag); - display.setCursor(0, 36); - display.println(F("E-STOP ACTIVE")); - display.setCursor(0, 50); - display.printf("Clears in %ds", - (int)((FALL_COOLDOWN_MS - (millis() - g_fall_last_ms)) / 1000)); + display.setCursor(20, 48); + display.println(F("RELEASE TO CLEAR")); display.display(); 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); + /* Discovery scan progress */ + if (g_disc_state == DISC_SCAN) { 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.println(F("ANCHOR SCAN")); + display.setCursor(0, 14); + display.printf("Probing %d / %d", g_disc_cursor, DISC_PROBE_IDS); + display.setCursor(0, 26); + display.printf("Found: %d anchor(s)", anchor_count_active()); + /* Progress bar */ + int bar_w = (g_disc_cursor * 126) / DISC_PROBE_IDS; + display.drawRect(0, 40, 128, 8, SSD1306_WHITE); + if (bar_w > 0) + display.fillRect(1, 41, bar_w, 6, SSD1306_WHITE); + display.setCursor(0, 54); + display.println(F("BTN0 = e-stop")); display.display(); return; } - /* BLE connected indicator */ - if (g_ble_connected) { - display.setTextSize(1); + uint32_t now = millis(); + int n_active = anchor_count_active(); + + /* No anchors found */ + if (n_active == 0) { + display.setTextSize(2); display.setTextColor(SSD1306_WHITE); - display.setCursor(88, 0); - display.print(F("[BLE]")); + display.setCursor(0, 0); + display.println(F("NO ANCHOR")); + display.setTextSize(1); + display.setCursor(0, 22); + uint32_t elapsed = now - g_last_rescan; + uint32_t next_s = (elapsed < DISC_RESCAN_MS) + ? (DISC_RESCAN_MS - elapsed) / 1000 : 0; + display.printf("Rescan in %lus", next_s); + display.setCursor(0, 36); + display.println(F("Check anchor power")); + display.setCursor(0, 54); + uint32_t secs = now / 1000; + display.printf("UP %02d:%02d seq:%d", secs / 60, secs % 60, g_seq); + display.display(); + return; } - uint32_t now = millis(); - + /* Normal: find closest active 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]; + for (int i = 0; i < DISC_PROBE_IDS; i++) { + if (g_anchors[i].active && g_anchors[i].range_mm > 0 && + g_anchors[i].range_mm < min_range) + min_range = g_anchors[i].range_mm; } + /* Line 1: big distance + anchor count badge */ display.setTextSize(3); display.setTextColor(SSD1306_WHITE); display.setCursor(0, 0); if (min_range < INT32_MAX && min_range > 0) { float m = min_range / 1000.0f; - if (m < 10.0f) - display.printf("%.1fm", m); - else - display.printf("%.0fm", m); + if (m < 10.0f) display.printf("%.1fm", m); + else display.printf("%.0fm", m); } else { display.println(F("---")); } + display.setTextSize(1); + display.setCursor(104, 2); + display.printf("%dA", n_active); + /* Line 2: per-anchor ranges */ display.setTextSize(1); display.setCursor(0, 28); - for (int i = 0; i < NUM_ANCHORS; i++) { - if (g_anchor_range_mm[i] > 0) { - display.printf("A%d:%.1fm ", i, g_anchor_range_mm[i] / 1000.0f); - } else { + for (int i = 0; i < DISC_PROBE_IDS; i++) { + if (!g_anchors[i].present) continue; + if (g_anchors[i].active) + display.printf("A%d:%.1fm ", i, g_anchors[i].range_mm / 1000.0f); + else display.printf("A%d:--- ", i); - } } + /* Line 3: link status + RSSI bars */ display.setCursor(0, 40); - if (g_imu_ok) { - display.printf("%.1fg", g_imu.accel_mag); - 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); - display.setCursor(100, 40); - display.printf("%+.0f", g_imu.gz); - } else { - 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) { - any_linked = true; - break; - } + bool any_fresh = false; + float best_rssi = -100.0f; + for (int i = 0; i < DISC_PROBE_IDS; i++) { + if (g_anchors[i].active && (now - g_anchors[i].last_ok_ms) < 2000) { + any_fresh = true; + if (g_anchors[i].rssi > best_rssi) best_rssi = g_anchors[i].rssi; } - display.println(any_linked ? F("LINKED") : F("SEARCHING...")); + } + if (any_fresh) { + int bars = constrain((int)((best_rssi + 90.0f) / 12.0f), 0, 5); + display.print(F("LINKED ")); + for (int b = 0; b < 5; b++) { + int x = 50 + b * 6, h = 2 + b * 2, y = 50 - h; + if (b < bars) display.fillRect(x, y, 4, h, SSD1306_WHITE); + else display.drawRect(x, y, 4, h, SSD1306_WHITE); + } + display.printf(" %.0fdB", best_rssi); + } else { + display.println(F("STALE -- searching --")); } + /* Line 4: uptime */ display.setCursor(0, 54); uint32_t secs = now / 1000; - display.printf("UP%02d:%02d p:%d", secs / 60, secs % 60, g_seq); - if (g_imu_ok) { - display.setCursor(100, 54); - display.print(F("IMU")); - } + display.printf("UP %02d:%02d seq:%d", secs / 60, secs % 60, g_seq); display.display(); } -/* ── DW1000Ranging callbacks ────────────────────────────────────── */ - -static void newRange(void) { - uint16_t addr = DW1000Ranging.getDistantDevice()->getShortAddress(); - float range = DW1000Ranging.getDistantDevice()->getRange(); - float rxPow = DW1000Ranging.getDistantDevice()->getRXPower(); - - if (range < 0.0f || range > 250.0f) return; - - int idx = anchor_index(addr); - int32_t range_mm = (int32_t)(range * 1000.0f + 0.5f); - - if (idx >= 0) { - g_anchor_range_mm[idx] = range_mm; - g_anchor_rssi[idx] = rxPow; - g_anchor_last_ok[idx] = millis(); - } - - Serial.printf("+RANGE:%d,%04X,%ld,%.1f\r\n", - (idx >= 0 ? idx : 99), addr, (long)range_mm, rxPow); - - espnow_send(MSG_RANGE, (uint8_t)(idx >= 0 ? idx : 0xFF), range_mm, rxPow); - - 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) { - Serial.printf("+NEW:%04X\r\n", device->getShortAddress()); -} - -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,142 +614,134 @@ 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 */ - 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); + Serial.printf("\r\n[uwb_tag] tag_id=0x%02X probe_ids=0..%d starting\r\n", + TAG_ID, DISC_PROBE_IDS - 1); /* OLED */ + Wire.begin(PIN_SDA, PIN_SCL); if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { - Serial.println("[uwb_tag] WARN: SSD1306 not found"); + Serial.println("[uwb_tag] WARN: SSD1306 not found — running headless"); } else { display.clearDisplay(); - display.ssd1306_command(SSD1306_DISPLAYON); // Turn display ON! - display.ssd1306_command(SSD1306_SETCONTRAST); - display.ssd1306_command(g_cfg.display_brightness); display.setTextSize(2); display.setTextColor(SSD1306_WHITE); display.setCursor(0, 0); display.println(F("SaltyBot")); display.setTextSize(1); display.setCursor(0, 20); - display.printf("Tag 0x%02X", TAG_ID); + display.printf("Tag 0x%02X v2.1", TAG_ID); display.setCursor(0, 35); - display.println(F("DW1000 + BLE + PM")); + display.println(F("DW3000 DS-TWR + ESP-NOW")); display.setCursor(0, 50); - display.printf("BLE: %s", g_ble_device_name); + display.println(F("Scanning anchors...")); display.display(); + Serial.println("[uwb_tag] OLED ok"); } - /* 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) - Serial.println("[uwb_tag] MPU6050 init OK (±8g, ±500dps)"); - else - Serial.println("[uwb_tag] MPU6050 not found — running without IMU"); - /* ESP-NOW */ WiFi.mode(WIFI_STA); WiFi.disconnect(); esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE); - if (esp_now_init() == ESP_OK) { - esp_now_peer_info_t peer = {}; - memcpy(peer.peer_addr, broadcast_mac, 6); - peer.channel = 0; - peer.encrypt = false; - esp_now_add_peer(&peer); - esp_now_register_recv_cb(espnow_rx_cb); - Serial.println("[uwb_tag] ESP-NOW tx+rx ok"); - } else { - Serial.println("[uwb_tag] WARN: ESP-NOW failed"); + if (esp_now_init() != ESP_OK) { + Serial.println("[uwb_tag] FATAL: esp_now_init failed"); + for (;;) delay(1000); + } + esp_now_peer_info_t peer = {}; + memcpy(peer.peer_addr, broadcast_mac, 6); + peer.channel = 0; + peer.encrypt = false; + esp_now_add_peer(&peer); + Serial.println("[uwb_tag] ESP-NOW ok"); + + /* DW3000 */ + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + pinMode(PIN_RST, OUTPUT); + digitalWrite(PIN_RST, LOW); + delay(2); + pinMode(PIN_RST, INPUT_PULLUP); + delay(5); + + if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) { + Serial.println("[uwb_tag] FATAL: DW3000 probe failed"); + for (;;) delay(1000); + } + if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) { + Serial.println("[uwb_tag] FATAL: dwt_initialise failed"); + for (;;) delay(1000); + } + if (dwt_configure(&dw_cfg) != DWT_SUCCESS) { + Serial.println("[uwb_tag] FATAL: dwt_configure failed"); + for (;;) delay(1000); } - /* BLE */ - ble_init(); + dwt_setrxantennadelay(ANT_DELAY); + dwt_settxantennadelay(ANT_DELAY); + dwt_settxpower(0x0E080222UL); - /* DW1000 */ - SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI); - dw1000_ranging_init(); + dwt_setcallbacks(cb_tx_done, cb_rx_ok, cb_rx_to, cb_rx_err, + nullptr, nullptr, nullptr); + dwt_setinterrupt( + DWT_INT_TXFRS | DWT_INT_RFCG | DWT_INT_RFTO | + DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_ARFE | DWT_INT_CPERR, + 0, DWT_ENABLE_INT_ONLY); + attachInterrupt(digitalPinToInterrupt(PIN_IRQ), + []() { dwt_isr(); }, RISING); - /* Init anchor state */ - for (int i = 0; i < NUM_ANCHORS; i++) { - g_anchor_range_mm[i] = -1; - g_anchor_rssi[i] = -100.0f; - g_anchor_last_ok[i] = 0; - } + Serial.printf("[uwb_tag] DW3000 ready ch=%d 6.8Mbps tag=0x%02X\r\n", + dw_cfg.chan, TAG_ID); + Serial.println("[uwb_tag] Starting anchor discovery..."); - 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); + disc_start_scan(); /* begin probing on startup */ } -/* ── Loop ───────────────────────────────────────────────────────── */ - -static uint32_t g_last_hb = 0; -static uint32_t g_last_imu = 0; +/* ── Main loop ──────────────────────────────────────────────────── */ void loop(void) { - /* 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(); - } + static uint32_t last_range_ms = 0; + static uint32_t last_hb_ms = 0; + static uint8_t range_cursor = 0; estop_check(); + if (g_estop_active) { + display_update(); + return; + } + + uint32_t now = millis(); /* Heartbeat every 1s */ - if (millis() - g_last_hb >= 1000) { + if (now - last_hb_ms >= 1000) { espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f); - g_last_hb = millis(); + last_hb_ms = 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(); + /* One ranging slot per RANGE_INTERVAL_MS */ + if (now - last_range_ms >= RANGE_INTERVAL_MS) { + last_range_ms = now; - 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, - g_imu.accel_mag); - s_last_imu_tx = millis(); + if (g_disc_state == DISC_SCAN) { + /* Discovery: probe one ID this slot */ + disc_scan_step(); + } else { + /* Normal: round-robin through active anchors */ + int tried = 0; + while (tried < DISC_PROBE_IDS) { + uint8_t id = range_cursor % DISC_PROBE_IDS; + range_cursor++; + if (g_anchors[id].active) { + range_and_update(id); + break; + } + tried++; + } + + /* Re-scan if table is empty or rescan interval elapsed */ + if (anchor_count_active() == 0 || + (now - g_last_rescan) >= DISC_RESCAN_MS) { + disc_start_scan(); + } + } } - /* 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) */ - pm_update(); - - /* Display at 5 Hz */ display_update(); - - delay(1); }