From 5e591ab46673433111f94b264cd74d3f79c19e2a Mon Sep 17 00:00:00 2001 From: Salty Bead Date: Mon, 30 Mar 2026 09:23:00 -0400 Subject: [PATCH] feat(uwb): replace ESP-NOW with DW1000 data frames + BLE GPS/IMU input - Tag: strip all ESP-NOW/WiFi code - Tag: add BLE GPS characteristic (def3) for iPhone binary GPS writes - Tag: add BLE Phone IMU characteristic (def4) for iPhone IMU writes - Tag: transmit GPS/IMU/heartbeat via DW1000 data frames (v2 protocol) - Tag: update OLED display with GPS speed/heading/fix indicators - Tag: e-stop now sent via UWB data frames (3x for reliability) - Anchor: strip all ESP-NOW/WiFi code - Anchor: receive DW1000 data frames, forward to serial as +GPS/+PIMU/+TIMU/+ESTOP - Protocol v2: magic {0x5B, 0x02}, msg types 0x60-0x64 --- TASK.md | 176 ++++ esp32/uwb_anchor/src/main.cpp | 307 ++++--- esp32/uwb_tag/src/main.cpp | 1490 +++++++++++++++++++++------------ 3 files changed, 1296 insertions(+), 677 deletions(-) create mode 100644 TASK.md diff --git a/TASK.md b/TASK.md new file mode 100644 index 0000000..bba9745 --- /dev/null +++ b/TASK.md @@ -0,0 +1,176 @@ +# Task: UWB Tag + Anchor Firmware Rewrite — GPS Breadcrumb Trail + +## Context +SaltyBot is a self-balancing robot that follows a rider on an EUC (electric unicycle). The rider wears a UWB tag (ESP32 UWB Pro with Display). The robot has 2 UWB anchors. The rider's iPhone connects to the tag via BLE and streams GPS+IMU data. The tag relays this over UWB data frames to the anchors on the robot, which forward to the Jetson Orin via serial. + +When the rider moves out of UWB range, the iPhone falls back to sending GPS over Tailscale/MQTT directly to Orin. The robot replays the exact breadcrumb path. + +## Files to Modify +- `esp32/uwb_tag/src/main.cpp` — Tag firmware (wearable, ESP32 UWB Pro with Display) +- `esp32/uwb_anchor/src/main.cpp` — Anchor firmware (on robot, ESP32 UWB Pro, no display) + +## Changes Required + +### Both Files +1. **Remove ALL ESP-NOW code** — no more WiFi, esp_now, broadcast MAC, ESP-NOW packets, etc. +2. Replace ESP-NOW transport with **DW1000 data frames** for tag→anchor communication + +### Tag Firmware (uwb_tag/src/main.cpp) + +#### Strip +- All ESP-NOW includes, init, send, receive, packet structs +- WiFi mode/channel/disconnect calls +- `espnow_send()`, `espnow_send_imu()`, `espnow_rx_cb()` + +#### Add: BLE GPS Input Service +New GATT characteristic for iPhone to write GPS data: +- **GPS Characteristic UUID:** `12345678-1234-5678-1234-56789abcdef3` (WRITE, WRITE_NR) +- iPhone writes a packed binary struct (not JSON — too slow at 5Hz): + +```c +#pragma pack(push, 1) +struct GpsInput { + int32_t lat_e7; // latitude * 1e7 (nanodegrees) + int32_t lon_e7; // longitude * 1e7 + int16_t alt_dm; // altitude in decimeters (±3276.7m range) + uint16_t speed_cmps; // ground speed in cm/s (0-655 m/s) + uint16_t heading_cd; // heading in centidegrees (0-35999) + uint8_t accuracy_dm; // horizontal accuracy in decimeters (0-25.5m) + uint8_t fix_type; // 0=none, 1=2D, 2=3D + uint32_t timestamp_ms; // iPhone monotonic timestamp +}; // 20 bytes total +#pragma pack(pop) +``` + +- On BLE write → store in `g_gps` global, set `g_gps_fresh = true` +- Also add an **iPhone IMU Characteristic UUID:** `12345678-1234-5678-1234-56789abcdef4` (WRITE, WRITE_NR) + +```c +#pragma pack(push, 1) +struct PhoneImuInput { + int16_t ax_mg; // acceleration X in milli-g + int16_t ay_mg; // Y + int16_t az_mg; // Z + int16_t gx_cdps; // gyro X in centi-degrees/sec + int16_t gy_cdps; // Y + int16_t gz_cdps; // Z + int16_t mx_ut; // magnetometer X in micro-Tesla (for heading) + int16_t my_ut; // Y + int16_t mz_ut; // Z + uint32_t timestamp_ms; // iPhone monotonic timestamp +}; // 22 bytes +#pragma pack(pop) +``` + +#### Add: DW1000 Data Frame TX +After each GPS or IMU BLE write, transmit a DW1000 data frame to the anchors: + +```c +#define UWB_MSG_GPS 0x60 +#define UWB_MSG_PIMU 0x61 // Phone IMU +#define UWB_MSG_TIMU 0x62 // Tag onboard IMU (MPU6050) + +#pragma pack(push, 1) +struct UwbDataFrame { + uint8_t magic[2]; // {0x5B, 0x02} — v2 protocol + uint8_t tag_id; + uint8_t msg_type; // UWB_MSG_GPS, UWB_MSG_PIMU, UWB_MSG_TIMU + uint8_t seq; + uint8_t payload[0]; // variable: GpsInput (20B) or ImuPayload (22B) or TagImu (14B) +}; +#pragma pack(pop) +``` + +Use `DW1000.setData()` + `DW1000.startTransmit()` to send raw data frames between ranging cycles. The DW1000Ranging library handles TWR, but we can interleave data frames in between. Use a simple approach: +- After each successful TWR cycle in `newRange()`, if there's fresh GPS/IMU data, transmit a data frame +- Also send GPS at 5Hz and tag IMU at 10Hz on a timer if no ranging is happening + +For the tag's own MPU6050 IMU, keep sending that too: +```c +#pragma pack(push, 1) +struct TagImuPayload { + 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 flags; // bit0: fall detected +}; // 14 bytes +#pragma pack(pop) +``` + +#### Update OLED Display +- Add GPS info line: lat/lon or speed+heading +- Show BLE data status (receiving from phone or not) +- Show "GPS: 3D" or "GPS: ---" indicator + +#### Keep Everything Else +- UWB ranging (DW1000Ranging as tag/initiator) +- BLE config service (existing UUIDs ...def0, ...def1, ...def2) +- MPU6050 IMU reads + fall detection +- Power management (display sleep, DW1000 sleep, deep sleep) +- E-stop on fall detection (send via UWB data frame instead of ESP-NOW) + +### Anchor Firmware (uwb_anchor/src/main.cpp) + +#### Strip +- All ESP-NOW code (includes, init, rx callback, send, packet structs, ring buffer) +- WiFi mode/channel/disconnect +- `espnow_send_range()`, `espnow_rx_cb()`, `espnow_process()`, `g_other_*` tracking + +#### Add: DW1000 Data Frame RX +- Register a DW1000 receive handler that catches data frames from the tag +- Parse UwbDataFrame header, then forward payload to Orin via serial: + +``` ++GPS:,,,,,,,,\r\n ++PIMU:,,,,,,,,,,\r\n ++TIMU:,,,,,,,,\r\n ++ESTOP:\r\n +``` + +#### Keep +- UWB ranging (DW1000Ranging as anchor/responder) +- AT command handler +- TDM slot system +- Serial output format for +RANGE lines + +## DW1000 Data Frame Implementation Notes + +The DW1000Ranging library uses the DW1000 for TWR (two-way ranging). To send data frames alongside ranging: + +**Option A (simpler):** Use the DW1000's transmit-after-ranging capability. After a TWR exchange completes, the tag can immediately send a data frame. The DW1000 library exposes `DW1000.setData()` and `DW1000.startTransmit()`. + +**Option B (more robust):** Use ESP-NOW on a separate WiFi channel just for data (not ranging). But we're removing ESP-NOW, so this is not an option. + +**Option C (recommended):** Since DW1000Ranging uses SPI and manages the transceiver state, the safest approach is to **interleave data TX between ranging cycles**. After `newRange()` fires and a TWR cycle completes, queue one data frame for TX. The tag can check `DW1000Ranging.loop()` return or use a flag to know when a cycle is done. + +Actually, the simplest reliable approach: **Use DW1000's user data field in ranging frames**. The DW1000Ranging library sends ranging frames that have a data payload area. We can embed our GPS/IMU data in the ranging frame itself. Check if `DW1000Ranging` exposes `setUserData()` or similar. If not, we can modify the ranging frame assembly. + +If none of that works cleanly, fall back to **periodic standalone TX** using a timer. Between ranging cycles, send raw DW1000 frames. The anchors listen for both ranging responses and raw data frames (differentiated by frame type or header byte). + +Pick whichever approach compiles cleanly with the existing DW1000/DW1000Ranging library in `../../lib/DW1000/`. + +## Important Constraints +- ESP32 (not ESP32-S3) — standard Arduino + ESP-IDF +- DW1000 library is in `../../lib/DW1000/` (Makerfabs fork from mf_DW1000.zip) +- PlatformIO build, `pio run -e tag` / `pio run -e anchor0` / `pio run -e anchor1` +- Keep both files self-contained (no shared header files needed — duplicate structs is fine) +- Tag CS pin = 21 (display board), Anchor CS pin = 4 (non-display board) +- OLED is 128x64 SSD1306 on I2C (tag only) +- BLE + DW1000 SPI coexist fine on ESP32 (different peripherals) +- Max DW1000 frame payload is ~127 bytes — our packets are well under that + +## Build Verification +After changes, verify the code compiles: +```bash +cd esp32/uwb_tag && pio run -e tag +cd esp32/uwb_anchor && pio run -e anchor0 +``` +(anchor0/anchor1 envs should exist in platformio.ini, or just `pio run`) + +## Branch +Working on branch: `salty/uwb-tag-display-wireless` +Commit changes with a descriptive message when done. diff --git a/esp32/uwb_anchor/src/main.cpp b/esp32/uwb_anchor/src/main.cpp index e15cc3a..b3f7e93 100644 --- a/esp32/uwb_anchor/src/main.cpp +++ b/esp32/uwb_anchor/src/main.cpp @@ -7,10 +7,13 @@ * * Serial output to Jetson (115200): * +RANGE:,,,\r\n + * +GPS:,,,,,,,,\r\n + * +PIMU:,,,,,,,,,,\r\n + * +TIMU:,,,,,,,,\r\n + * +ESTOP:,\r\n * - * Also receives ESP-NOW packets from tag and forwards: - * +ESPNOW:,,,,,,\r\n - * +ESTOP:\r\n (priority) + * Receives DW1000 data frames from tag (v2 protocol, magic 0x5B 0x02). + * Ranging via DW1000Ranging library (TWR responder). * * AT commands (host → anchor): * AT+ID? → +ID: @@ -23,10 +26,8 @@ #include #include #include -#include -#include -#include #include "DW1000Ranging.h" +#include "DW1000.h" /* ── Config ─────────────────────────────────────────────────────── */ @@ -36,7 +37,6 @@ #define SERIAL_BAUD 115200 -/* Unique DW1000 address per anchor (last 2 bytes differ) */ #if ANCHOR_ID == 0 static char ANCHOR_ADDR[] = "86:17:5B:D5:A9:9A:E2:00"; #else @@ -52,72 +52,83 @@ static char ANCHOR_ADDR[] = "86:17:5B:D5:A9:9A:E2:01"; #define PIN_RST 27 #define PIN_IRQ 34 -/* ── ESP-NOW packet format (shared with tag) ───────────────────── */ +/* ── UWB Data Frame Protocol (shared with tag) ──────────────────── */ -#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 UWB_MAGIC_0 0x5B +#define UWB_MAGIC_1 0x02 + +#define UWB_MSG_GPS 0x60 +#define UWB_MSG_PIMU 0x61 +#define UWB_MSG_TIMU 0x62 +#define UWB_MSG_ESTOP 0x63 +#define UWB_MSG_HB 0x64 #pragma pack(push, 1) -struct EspNowPacket { +struct UwbFrameHeader { uint8_t magic[2]; uint8_t tag_id; uint8_t msg_type; - uint8_t anchor_id; - int32_t range_mm; - float rssi_dbm; + uint8_t seq; +}; + +struct GpsInput { + int32_t lat_e7; + int32_t lon_e7; + int16_t alt_dm; + uint16_t speed_cmps; + uint16_t heading_cd; + uint8_t accuracy_dm; + uint8_t fix_type; uint32_t timestamp_ms; - uint8_t battery_pct; +}; + +struct PhoneImuInput { + int16_t ax_mg; + int16_t ay_mg; + int16_t az_mg; + int16_t gx_cdps; + int16_t gy_cdps; + int16_t gz_cdps; + int16_t mx_ut; + int16_t my_ut; + int16_t mz_ut; + uint32_t timestamp_ms; +}; + +struct TagImuPayload { + 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 flags; - uint8_t seq_num; - uint8_t _pad; }; #pragma pack(pop) -/* ISR ring buffer for ESP-NOW */ -#define ESPNOW_Q_SIZE 8 -static EspNowPacket g_enq[ESPNOW_Q_SIZE]; -static volatile int g_en_head = 0, g_en_tail = 0; +/* ── DW1000 data frame RX buffer ────────────────────────────────── */ -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 *p = (const EspNowPacket *)data; - if (p->magic[0] != ESPNOW_MAGIC_0 || p->magic[1] != ESPNOW_MAGIC_1) return; - int next = (g_en_head + 1) % ESPNOW_Q_SIZE; - if (next == g_en_tail) return; - memcpy(&g_enq[g_en_head], p, sizeof(EspNowPacket)); - g_en_head = next; -} +#define UWB_RX_BUF_SIZE 64 +#define UWB_RX_Q_SIZE 8 -/* ── ESP-NOW TX: broadcast own range to other anchor + tag ──────── */ +static uint8_t g_rx_q[UWB_RX_Q_SIZE][UWB_RX_BUF_SIZE]; +static uint16_t g_rx_q_len[UWB_RX_Q_SIZE]; +static volatile int g_rx_head = 0, g_rx_tail = 0; -static uint8_t broadcast_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; -static uint8_t g_seq = 0; +/* Last seen sequence per tag to detect duplicates */ +static uint8_t g_last_seq[256] = {}; /* indexed by tag_id */ +static bool g_seq_seen[256] = {}; -/* Track the other anchor's range (received via ESP-NOW) */ -static int32_t g_other_range_mm = -1; -static float g_other_rssi = -100.0f; -static uint32_t g_other_last_ms = 0; - -static void espnow_send_range(int32_t range_mm, float rssi, uint16_t tag_addr) { - EspNowPacket pkt = {}; - pkt.magic[0] = ESPNOW_MAGIC_0; - pkt.magic[1] = ESPNOW_MAGIC_1; - pkt.tag_id = (uint8_t)(tag_addr & 0xFF); - pkt.msg_type = MSG_RANGE; - pkt.anchor_id = ANCHOR_ID; - pkt.range_mm = range_mm; - pkt.rssi_dbm = rssi; - pkt.timestamp_ms = millis(); - pkt.battery_pct = 0xFF; - pkt.flags = 0x00; - pkt.seq_num = g_seq++; - esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); -} +/* + * DW1000 RX callback for data frames. + * Called from ISR context — keep it fast, just copy to ring buffer. + * + * Note: The DW1000Ranging library handles TWR frames internally. + * We register a separate handler for non-ranging frames via DW1000 low-level API. + * However, since DW1000Ranging manages the receiver state, we intercept + * data frames by polling in loop() between ranging cycles. + */ /* ── AT command handler ─────────────────────────────────────────── */ @@ -146,58 +157,107 @@ static void serial_poll(void) { } } -/* ── ESP-NOW processing ─────────────────────────────────────────── */ +/* ── Process received UWB data frames ────────────────────────────── */ -static void espnow_process(void) { - while (g_en_tail != g_en_head) { - const EspNowPacket &pkt = (const EspNowPacket &)g_enq[g_en_tail]; - g_en_tail = (g_en_tail + 1) % ESPNOW_Q_SIZE; +static void process_uwb_data(const uint8_t *data, uint16_t len) { + if (len < sizeof(UwbFrameHeader)) return; - if (pkt.msg_type == MSG_ESTOP) { - if (pkt.flags & 0x01) - Serial.printf("+ESTOP:%d\r\n", pkt.tag_id); - else - Serial.printf("+ESTOP_CLEAR:%d\r\n", pkt.tag_id); - continue; + const UwbFrameHeader *hdr = (const UwbFrameHeader *)data; + if (hdr->magic[0] != UWB_MAGIC_0 || hdr->magic[1] != UWB_MAGIC_1) return; + + uint8_t tag = hdr->tag_id; + uint8_t mtype = hdr->msg_type; + uint8_t seq = hdr->seq; + const uint8_t *payload = data + sizeof(UwbFrameHeader); + uint16_t plen = len - sizeof(UwbFrameHeader); + + /* Dedup: skip if we've seen this exact seq from this tag */ + if (g_seq_seen[tag] && g_last_seq[tag] == seq) return; + g_last_seq[tag] = seq; + g_seq_seen[tag] = true; + + switch (mtype) { + case UWB_MSG_GPS: { + if (plen < sizeof(GpsInput)) return; + const GpsInput *gps = (const GpsInput *)payload; + Serial.printf("+GPS:%d,%ld,%ld,%d,%u,%u,%u,%u,%lu\r\n", + tag, + (long)gps->lat_e7, (long)gps->lon_e7, + gps->alt_dm, gps->speed_cmps, gps->heading_cd, + gps->accuracy_dm, gps->fix_type, + (unsigned long)gps->timestamp_ms); + break; + } + case UWB_MSG_PIMU: { + if (plen < sizeof(PhoneImuInput)) return; + const PhoneImuInput *pi = (const PhoneImuInput *)payload; + Serial.printf("+PIMU:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%lu\r\n", + tag, + pi->ax_mg, pi->ay_mg, pi->az_mg, + pi->gx_cdps, pi->gy_cdps, pi->gz_cdps, + pi->mx_ut, pi->my_ut, pi->mz_ut, + (unsigned long)pi->timestamp_ms); + break; + } + case UWB_MSG_TIMU: { + if (plen < sizeof(TagImuPayload)) return; + const TagImuPayload *ti = (const TagImuPayload *)payload; + Serial.printf("+TIMU:%d,%d,%d,%d,%d,%d,%d,%u,%u\r\n", + tag, + ti->ax_mg, ti->ay_mg, ti->az_mg, + ti->gx_dps10, ti->gy_dps10, ti->gz_dps10, + ti->accel_mag_10, ti->flags); + break; + } + case UWB_MSG_ESTOP: { + if (plen < sizeof(TagImuPayload)) { + /* Minimal e-stop with just flags byte */ + uint8_t flags = (plen >= 1) ? payload[plen - 1] : 0x01; + Serial.printf("+ESTOP:%d,%u\r\n", tag, flags); + } else { + const TagImuPayload *ep = (const TagImuPayload *)payload; + Serial.printf("+ESTOP:%d,%u\r\n", tag, ep->flags); } + break; + } + case UWB_MSG_HB: { + /* Heartbeat — just note tag is alive */ + Serial.printf("+HB:%d\r\n", tag); + break; + } + default: + Serial.printf("+UWB_UNK:%d,0x%02X,%d\r\n", tag, mtype, plen); + break; + } +} - /* If this is a range from the OTHER anchor, track it and print as +RANGE */ - if (pkt.msg_type == MSG_RANGE && pkt.anchor_id != ANCHOR_ID - && pkt.anchor_id < 2) { - g_other_range_mm = pkt.range_mm; - g_other_rssi = pkt.rssi_dbm; - g_other_last_ms = millis(); - /* Print as +RANGE so Orin sees both anchors from either serial port */ - Serial.printf("+RANGE:%d,%02X,%ld,%.1f\r\n", - pkt.anchor_id, pkt.tag_id, - (long)pkt.range_mm, pkt.rssi_dbm); - continue; - } +/* + * Try to read a data frame from DW1000. + * Called between ranging cycles when the DW1000 might have buffered + * a non-ranging frame. We briefly switch to receive mode, check for + * data, then let ranging resume. + * + * This is a cooperative approach: the DW1000Ranging library uses + * the transceiver for TWR, and we steal idle slots to listen for + * data frames from the tag. + */ +static uint8_t g_uwb_rx_buf[UWB_RX_BUF_SIZE]; - /* Forward IMU and FALL packets to Jetson serial */ - if (pkt.msg_type == MSG_IMU || pkt.msg_type == MSG_FALL) { - const uint8_t *raw = (const uint8_t *)&pkt; - Serial.printf("+%s:%d,", - pkt.msg_type == MSG_FALL ? "FALL" : "IMU", - pkt.tag_id); - /* Forward as hex for easy parsing: ax,ay,az,gx,gy,gz,mag */ - int16_t ax = (int16_t)(raw[4] | (raw[5] << 8)); - int16_t ay = (int16_t)(raw[6] | (raw[7] << 8)); - int16_t az = (int16_t)(raw[8] | (raw[9] << 8)); - int16_t gx = (int16_t)(raw[10] | (raw[11] << 8)); - int16_t gy = (int16_t)(raw[12] | (raw[13] << 8)); - int16_t gz = (int16_t)(raw[14] | (raw[15] << 8)); - uint8_t mag = raw[16]; - Serial.printf("%.3f,%.3f,%.3f,%.1f,%.1f,%.1f,%.1f\r\n", - ax/1000.0f, ay/1000.0f, az/1000.0f, - gx/10.0f, gy/10.0f, gz/10.0f, - mag/10.0f); - continue; - } +static void uwb_check_data_rx(void) { + /* Check if DW1000 received a frame that DW1000Ranging didn't consume */ + if (!DW1000.isReceiveDone()) return; - Serial.printf("+ESPNOW:%d,%02X,%ld,%.1f,%d,%02X,%d\r\n", - pkt.tag_id, pkt.msg_type, (long)pkt.range_mm, - pkt.rssi_dbm, pkt.battery_pct, pkt.flags, pkt.seq_num); + uint16_t len = DW1000.getDataLength(); + if (len < sizeof(UwbFrameHeader) || len > UWB_RX_BUF_SIZE) { + /* Not our frame or too large — let ranging handle it */ + return; + } + + DW1000.getData(g_uwb_rx_buf, len); + + /* Quick magic check — only process v2 data frames */ + if (g_uwb_rx_buf[0] == UWB_MAGIC_0 && g_uwb_rx_buf[1] == UWB_MAGIC_1) { + process_uwb_data(g_uwb_rx_buf, len); } } @@ -214,9 +274,6 @@ static void newRange(void) { Serial.printf("+RANGE:%d,%04X,%ld,%.1f\r\n", ANCHOR_ID, tag_addr, (long)range_mm, rxPow); - - /* Broadcast own range via ESP-NOW → other anchor + tag receive it */ - espnow_send_range(range_mm, rxPow, tag_addr); } static void newBlink(DW1000Device *device) { @@ -232,26 +289,10 @@ static void inactiveDevice(DW1000Device *device) { void setup(void) { Serial.begin(SERIAL_BAUD); delay(300); - Serial.printf("\r\n[uwb_anchor] id=%d addr=%s starting\r\n", ANCHOR_ID, ANCHOR_ADDR); + Serial.printf("\r\n[uwb_anchor] id=%d addr=%s v2 (UWB data frames)\r\n", + ANCHOR_ID, ANCHOR_ADDR); - /* ESP-NOW receiver */ - WiFi.mode(WIFI_STA); - WiFi.disconnect(); - esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE); - if (esp_now_init() == ESP_OK) { - esp_now_register_recv_cb(espnow_rx_cb); - /* Add broadcast peer for TX */ - 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_anchor] ESP-NOW rx+tx ok"); - } else { - Serial.println("[uwb_anchor] WARN: ESP-NOW failed"); - } - - /* DW1000 */ + /* DW1000 ranging */ SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI); DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ); @@ -262,26 +303,28 @@ void setup(void) { DW1000Ranging.startAsAnchor(ANCHOR_ADDR, DW1000.MODE_LONGDATA_RANGE_LOWPOWER, false); Serial.printf("[uwb_anchor] DW1000 ready id=%d MODE_LONGDATA_RANGE_LOWPOWER\r\n", ANCHOR_ID); - Serial.println("[uwb_anchor] Listening for tags..."); + Serial.println("[uwb_anchor] Listening for tags + UWB data frames..."); } /* ── Loop ───────────────────────────────────────────────────────── */ /* - * Time-division multiplexing: 2 anchors share the same channel. - * Each anchor only responds during its time slot to avoid RF collisions. - * Slot duration = 50ms → each anchor gets 10 Hz effective rate. - * Anchor 0 responds during even slots, anchor 1 during odd slots. + * TDM: 2 anchors share the same channel. + * Each anchor only responds during its time slot. + * Slot = 50ms → each anchor gets 10 Hz effective rate. + * Between slots, check for UWB data frames. */ #define SLOT_MS 50 #define NUM_ANCHORS_TOTAL 2 void loop(void) { serial_poll(); - espnow_process(); uint32_t slot = (millis() / SLOT_MS) % NUM_ANCHORS_TOTAL; if (slot == ANCHOR_ID) { DW1000Ranging.loop(); } + + /* Check for UWB data frames from tag between ranging slots */ + uwb_check_data_rx(); } diff --git a/esp32/uwb_tag/src/main.cpp b/esp32/uwb_tag/src/main.cpp index 1b8e10f..81db9d1 100644 --- a/esp32/uwb_tag/src/main.cpp +++ b/esp32/uwb_tag/src/main.cpp @@ -1,92 +1,71 @@ /* - * uwb_tag — SaltyBot ESP32 UWB Pro tag firmware (DS-TWR initiator) - * Issue #545 + display/ESP-NOW/e-stop extensions - * Issue #698: anchor auto-discovery + * uwb_tag — SaltyBot ESP32 UWB Pro with Display (DW1000 TWR initiator) * - * Hardware: Makerfabs ESP32 UWB Pro with Display (DW3000 + SSD1306 OLED) + * Hardware: Makerfabs ESP32 UWB Pro with Display + * DW1000 (200m range), SSD1306 128x64 OLED, LiPo battery + * MPU6050 (accel+gyro) on shared I2C bus @0x68 * - * 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). + * Worn by rider on EUC while robot follows. + * iPhone connects via BLE and streams GPS + IMU data. + * Tag relays GPS/IMU to anchors on robot via DW1000 data frames. + * Robot replays the exact breadcrumb path at its own pace. * - * 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". + * OLED display (5 Hz): + * Line 1: Big distance to nearest anchor + * Line 2: Per-anchor ranges + * Line 3: GPS speed+heading or IMU accel + * Line 4: GPS fix + uptime + BLE status * - * 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. + * DW1000 data frames (interleaved with TWR): + * GPS from iPhone at 5 Hz + * Phone IMU at 10 Hz + * Tag IMU (MPU6050) at 10 Hz + * E-stop/fall at max rate * - * Serial output (USB, 115200) - * ──────────────────────────── - * +RANGE:,,\r\n - * +DISC:FOUND,\r\n — anchor added to table - * +DISC:LOST,\r\n — anchor removed from table + * 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 + * LED: GPIO 2 * - * 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) + * Power management (Issue #689): + * OLED auto-off after sleep_timeout_s inactivity + * DW1000 deep sleep after 5min idle + * ESP32 deep sleep on GPIO0 hold 3s * - * 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) + * BLE Services: + * Config (existing): UUIDs ...def0/def1/def2 + * GPS Input: ...def3 (WRITE) — iPhone writes GpsInput struct + * Phone IMU: ...def4 (WRITE) — iPhone writes PhoneImuInput struct */ #include #include #include -#include -#include -#include -#include - -#include "dw3000.h" +#include +#include +#include +#include +#include +#include +#include +#include "DW1000Ranging.h" +#include "DW1000.h" #include #include -/* ── Configurable ───────────────────────────────────────────────── */ +/* ── Config ─────────────────────────────────────────────────────── */ #ifndef TAG_ID -# define TAG_ID 0x01 +# define TAG_ID 0x01 #endif -#ifndef RANGE_INTERVAL_MS -# define RANGE_INTERVAL_MS 50 /* 20 Hz round-robin */ -#endif +static char TAG_ADDR[] = "7D:00:22:EA:82:60:3B:9B"; -#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 */ +#define SERIAL_BAUD 115200 +#define NUM_ANCHORS 2 /* ── Pins ───────────────────────────────────────────────────────── */ @@ -101,509 +80,911 @@ #define PIN_SCL 5 #define PIN_LED 2 -#define PIN_ESTOP 0 /* BOOT button, active LOW */ +#define PIN_ESTOP 0 /* ── OLED ───────────────────────────────────────────────────────── */ -#define SCREEN_W 128 -#define SCREEN_H 64 -Adafruit_SSD1306 display(SCREEN_W, SCREEN_H, &Wire, -1); +Adafruit_SSD1306 display(128, 64, &Wire, -1); -/* ── ESP-NOW ────────────────────────────────────────────────────── */ +/* ── UWB Data Frame Protocol ────────────────────────────────────── */ -#define ESPNOW_MAGIC_0 0x5B -#define ESPNOW_MAGIC_1 0x01 +#define UWB_MAGIC_0 0x5B +#define UWB_MAGIC_1 0x02 /* v2 — DW1000 data frames (replaces ESP-NOW v1) */ -#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; +#define UWB_MSG_GPS 0x60 +#define UWB_MSG_PIMU 0x61 /* Phone IMU (from iPhone via BLE) */ +#define UWB_MSG_TIMU 0x62 /* Tag onboard IMU (MPU6050) */ +#define UWB_MSG_ESTOP 0x63 +#define UWB_MSG_HB 0x64 /* Heartbeat */ #pragma pack(push, 1) -struct EspNowPacket { - uint8_t magic[2]; +struct UwbFrameHeader { + uint8_t magic[2]; /* {0x5B, 0x02} */ uint8_t tag_id; uint8_t msg_type; - uint8_t anchor_id; - int32_t range_mm; - float rssi_dbm; - uint32_t timestamp_ms; - uint8_t battery_pct; - uint8_t flags; - uint8_t seq_num; - uint8_t _pad; + uint8_t seq; }; + +struct GpsInput { + int32_t lat_e7; /* latitude * 1e7 */ + int32_t lon_e7; /* longitude * 1e7 */ + int16_t alt_dm; /* altitude in decimeters */ + uint16_t speed_cmps; /* ground speed cm/s */ + uint16_t heading_cd; /* heading centidegrees 0-35999 */ + uint8_t accuracy_dm; /* horizontal accuracy decimeters */ + uint8_t fix_type; /* 0=none, 1=2D, 2=3D */ + uint32_t timestamp_ms; /* iPhone monotonic ms */ +}; /* 20 bytes */ + +struct PhoneImuInput { + int16_t ax_mg; + int16_t ay_mg; + int16_t az_mg; + int16_t gx_cdps; /* centi-degrees/sec */ + int16_t gy_cdps; + int16_t gz_cdps; + int16_t mx_ut; /* micro-Tesla */ + int16_t my_ut; + int16_t mz_ut; + uint32_t timestamp_ms; +}; /* 22 bytes */ + +struct TagImuPayload { + 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 flags; /* bit0: fall detected */ +}; /* 14 bytes */ #pragma pack(pop) -static_assert(sizeof(EspNowPacket) == 20, "packet must be 20 bytes"); +/* Max DW1000 frame: header(5) + largest payload(22 for PhoneImu) = 27 bytes. Well under 127. */ +#define UWB_TX_BUF_SIZE 48 +static uint8_t g_uwb_tx_buf[UWB_TX_BUF_SIZE]; +static uint8_t g_uwb_seq = 0; -/* ── Anchor table ───────────────────────────────────────────────── */ +/* ── UWB Data TX ────────────────────────────────────────────────── */ -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 */ +static bool g_ranging_active = false; /* true while DW1000Ranging owns the transceiver */ +static bool g_uwb_tx_pending = false; +static uint8_t g_uwb_tx_len = 0; + +/* + * Queue a data frame for TX. Actual transmission happens in loop() + * between ranging cycles to avoid SPI contention. + */ +static void uwb_queue_frame(uint8_t msg_type, const void *payload, uint8_t payload_len) { + if (payload_len + sizeof(UwbFrameHeader) > UWB_TX_BUF_SIZE) return; + + UwbFrameHeader *hdr = (UwbFrameHeader *)g_uwb_tx_buf; + hdr->magic[0] = UWB_MAGIC_0; + hdr->magic[1] = UWB_MAGIC_1; + hdr->tag_id = TAG_ID; + hdr->msg_type = msg_type; + hdr->seq = g_uwb_seq++; + + memcpy(g_uwb_tx_buf + sizeof(UwbFrameHeader), payload, payload_len); + g_uwb_tx_len = sizeof(UwbFrameHeader) + payload_len; + g_uwb_tx_pending = true; +} + +/* + * Transmit queued data frame via DW1000. + * Called from loop() when ranging is idle. + */ +static void uwb_tx_flush(void) { + if (!g_uwb_tx_pending || g_uwb_tx_len == 0) return; + + DW1000.newTransmit(); + DW1000.setDefaults(); + DW1000.setData(g_uwb_tx_buf, g_uwb_tx_len); + DW1000.startTransmit(); + + g_uwb_tx_pending = false; + g_uwb_tx_len = 0; +} + +/* ── 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 + +static bool g_imu_ok = false; + +struct ImuData { + float ax, ay, az; + float gx, gy, gz; + float accel_mag; }; -static AnchorEntry g_anchors[DISC_PROBE_IDS] = {}; +static ImuData g_imu = {}; -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; +#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(); } -/* ── Discovery state ────────────────────────────────────────────── */ +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(); +} -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 */ +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); /* clock = PLL X gyro */ + delay(10); + mpu6050_write(MPU6050_ACCEL_CFG, 0x10); /* ±8g */ + mpu6050_write(MPU6050_GYRO_CFG, 0x08); /* ±500dps */ + return true; +} -/* ── E-Stop (also read by espnow_send flags byte) ───────────────── */ +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); -static bool g_estop_active = false; + 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); +} -/* ── DW3000 PHY config (must match anchor) ──────────────────────── */ +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; -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, + 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, (int)(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); + } +} + +/* ── GPS + Phone IMU state (written by iPhone via BLE) ──────────── */ + +static GpsInput g_gps = {}; +static bool g_gps_fresh = false; +static uint32_t g_gps_last_rx_ms = 0; + +static PhoneImuInput g_pimu = {}; +static bool g_pimu_fresh = false; +static uint32_t g_pimu_last_rx_ms = 0; + +/* ── BLE Configuration + Sensor Input ──────────────────────────── + * + * GATT layout: + * Service 12345678-1234-5678-1234-56789abcdef0 + * Config ...def1 [READ | WRITE] — JSON config (existing) + * Status ...def2 [READ | NOTIFY] — config status (existing) + * GPS ...def3 [WRITE | WRITE_NR] — iPhone GPS binary + * PhIMU ...def4 [WRITE | WRITE_NR] — iPhone IMU binary + * ─────────────────────────────────────────────────────────────── */ + +#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 BLE_GPS_UUID "12345678-1234-5678-1234-56789abcdef3" +#define BLE_PIMU_UUID "12345678-1234-5678-1234-56789abcdef4" + +#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; }; -/* ── Frame format ──────────────────────────────────────────────── */ +static UwbConfig g_cfg = {}; +static Preferences g_prefs; +static BLECharacteristic *g_ble_cfg_char = nullptr; +static BLECharacteristic *g_ble_sts_char = nullptr; +static BLECharacteristic *g_ble_gps_char = nullptr; +static BLECharacteristic *g_ble_pimu_char = nullptr; +static bool g_ble_connected = false; +static char g_ble_device_name[20] = {}; -#define FTYPE_POLL 0x01 -#define FTYPE_RESP 0x02 -#define FTYPE_FINAL 0x03 +/* ── NVS config persistence ──────────────────────────────────────── */ -#define FRAME_HDR 3 -#define FCS_LEN 2 +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(); -#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) + 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; -/* ── 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; + 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 ts_write(uint8_t *p, uint64_t v) { - for (int i = 0; i < 5; i++, v >>= 8) p[i] = (uint8_t)(v & 0xFF); +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 inline uint64_t ts_diff(uint64_t later, uint64_t earlier) { - return (later - earlier) & DWT_TS_MASK; +static void cfg_apply(void) { + display.ssd1306_command(SSD1306_SETCONTRAST); + display.ssd1306_command(g_cfg.display_brightness); } -static inline double ticks_to_s(uint64_t t) { - return (double)t / (128.0 * 499200000.0); +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"); } -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; +/* ── 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; } -/* ── ESP-NOW send ───────────────────────────────────────────────── */ +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 void espnow_send(uint8_t msg_type, uint8_t anchor_id, - int32_t range_mm, float rssi) { - EspNowPacket pkt = {}; - pkt.magic[0] = ESPNOW_MAGIC_0; - pkt.magic[1] = ESPNOW_MAGIC_1; - pkt.tag_id = TAG_ID; - pkt.msg_type = msg_type; - pkt.anchor_id = anchor_id; - pkt.range_mm = range_mm; - pkt.rssi_dbm = rssi; - pkt.timestamp_ms = millis(); - 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 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(); + } +}; + +static void ble_notify_status(const char *msg) { + if (g_ble_sts_char) { + g_ble_sts_char->setValue(msg); + g_ble_sts_char->notify(); + } +} + +class BleCfgCb : public BLECharacteristicCallbacks { + void onRead(BLECharacteristic *ch) override { + char buf[256]; + cfg_to_json(buf, sizeof(buf)); + ch->setValue(buf); + } + + void onWrite(BLECharacteristic *ch) override { + String val = ch->getValue(); + if (val.length() == 0) { + ble_notify_status("+ERR:empty payload"); + return; + } + 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); + return; + } + cfg_save(); + cfg_apply(); + ble_notify_status("+OK"); + } +}; + +/* ── BLE GPS + Phone IMU write callbacks ─────────────────────────── */ + +class BleGpsCb : public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *ch) override { + String val = ch->getValue(); + if (val.length() != sizeof(GpsInput)) { + Serial.printf("[ble] GPS bad len=%d (want %d)\r\n", + (int)val.length(), (int)sizeof(GpsInput)); + return; + } + memcpy(&g_gps, val.c_str(), sizeof(GpsInput)); + g_gps_fresh = true; + g_gps_last_rx_ms = millis(); + } +}; + +class BlePimuCb : public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *ch) override { + String val = ch->getValue(); + if (val.length() != sizeof(PhoneImuInput)) { + Serial.printf("[ble] PIMU bad len=%d (want %d)\r\n", + (int)val.length(), (int)sizeof(PhoneImuInput)); + return; + } + memcpy(&g_pimu, val.c_str(), sizeof(PhoneImuInput)); + g_pimu_fresh = true; + g_pimu_last_rx_ms = millis(); + } +}; + +/* ── 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(BLEUUID(BLE_SVC_UUID), 20); /* 20 handles for 4 chars */ + + /* Config characteristic (existing) */ + 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 (existing) */ + 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"); + + /* GPS input characteristic (NEW) */ + g_ble_gps_char = svc->createCharacteristic( + BLE_GPS_UUID, + BLECharacteristic::PROPERTY_WRITE | + BLECharacteristic::PROPERTY_WRITE_NR + ); + g_ble_gps_char->setCallbacks(new BleGpsCb()); + + /* Phone IMU input characteristic (NEW) */ + g_ble_pimu_char = svc->createCharacteristic( + BLE_PIMU_UUID, + BLECharacteristic::PROPERTY_WRITE | + BLECharacteristic::PROPERTY_WRITE_NR + ); + g_ble_pimu_char->setCallbacks(new BlePimuCb()); + + 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] GPS UUID: %s\r\n", BLE_GPS_UUID); + Serial.printf("[ble] PhoneIMU UUID: %s\r\n", BLE_PIMU_UUID); +} + +/* ── 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]; + +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 uint32_t g_estop_last_tx = 0; +static bool g_estop_active = false; static void estop_check(void) { - bool pressed = (digitalRead(PIN_ESTOP) == LOW); - - if (pressed && !g_estop_active) { + if (g_fall_detected && !g_estop_active) { g_estop_active = true; - Serial.println("+ESTOP:ACTIVE"); - } - - 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) { + Serial.println("+ESTOP:FALL"); + /* Send e-stop via UWB data frame (3x for reliability) */ + TagImuPayload estop_payload = {}; + estop_payload.ax_mg = (int16_t)(g_imu.ax * 1000.0f); + estop_payload.ay_mg = (int16_t)(g_imu.ay * 1000.0f); + estop_payload.az_mg = (int16_t)(g_imu.az * 1000.0f); + estop_payload.accel_mag_10 = (uint8_t)constrain((int)(g_imu.accel_mag * 10.0f), 0, 255); + estop_payload.flags = 0x01; /* fall detected */ for (int i = 0; i < 3; i++) { - g_estop_active = false; - espnow_send(MSG_ESTOP, 0xFF, 0, 0.0f); - delay(10); + uwb_queue_frame(UWB_MSG_ESTOP, &estop_payload, sizeof(estop_payload)); + uwb_tx_flush(); + delay(5); } + } + + if (g_estop_active && !g_fall_detected) { g_estop_active = false; Serial.println("+ESTOP:CLEAR"); + TagImuPayload clear_payload = {}; + clear_payload.flags = 0x00; + uwb_queue_frame(UWB_MSG_ESTOP, &clear_payload, sizeof(clear_payload)); } } -/* ── DS-TWR: one complete ranging cycle with one anchor ─────────── */ +/* ── Power Management ────────────────────────────────────────────── */ -static int32_t twr_range_once(uint8_t anchor_id) { +#define PM_DW1000_SLEEP_MS (300UL * 1000) +#define PM_DW1000_WAKE_PERIOD_MS (30UL * 1000) +#define PM_DW1000_WAKE_WINDOW_MS 5000 +#define PM_DEEP_SLEEP_HOLD_MS 3000 - /* 1. TX POLL */ - uint8_t poll[POLL_FRAME_LEN]; - poll[0] = FTYPE_POLL; - poll[1] = TAG_ID; - poll[2] = anchor_id; +static uint32_t g_pm_last_activity_ms = 0; +static bool g_pm_display_on = true; - 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 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; - 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 uint32_t g_btn_held_ms = 0; - uint32_t t0 = millis(); - while (!g_tx_done && millis() - t0 < 15) yield(); +static void dw1000_ranging_init(void); - uint8_t poll_tx_raw[5]; - dwt_readtxtimestamp(poll_tx_raw); - uint64_t T_poll_tx = ts_read(poll_tx_raw); +static void pm_activity(void) { + g_pm_last_activity_ms = millis(); - /* 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; + if (!g_pm_display_on) { + display.ssd1306_command(SSD1306_DISPLAYON); + g_pm_display_on = true; } - 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); } -/* ── Anchor table management ────────────────────────────────────── */ +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; +} -/* 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_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(); +} - if (range_mm > 0) { - float rssi = rx_power_dbm(); - bool was_active = a.active; +static void pm_enter_deep_sleep(void) { + Serial.println("[pm] ESP32 deep sleep — GPIO0 to wake"); + Serial.flush(); - a.present = true; - a.active = true; - a.range_mm = range_mm; - a.rssi = rssi; - a.last_ok_ms = millis(); - a.miss_count = 0; + 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); - Serial.printf("+RANGE:%d,%ld,%.1f\r\n", id, (long)range_mm, rssi); - espnow_send(MSG_RANGE, id, range_mm, rssi); + if (!g_dw1000_sleeping) { + DW1000.deepSleep(); + } - digitalWrite(PIN_LED, HIGH); - delay(2); - digitalWrite(PIN_LED, LOW); + esp_sleep_enable_ext0_wakeup(GPIO_NUM_0, 0); + esp_deep_sleep_start(); +} - if (!was_active) { - Serial.printf("+DISC:FOUND,%d\r\n", id); - espnow_send(MSG_DISC, id, range_mm, rssi); +static void pm_update(void) { + uint32_t now = millis(); + uint32_t idle_ms = now - g_pm_last_activity_ms; + + /* If receiving BLE data from phone, don't go idle */ + if (g_gps_last_rx_ms > 0 && (now - g_gps_last_rx_ms) < 5000) { + g_pm_last_activity_ms = now; + idle_ms = 0; + } + + /* OLED auto-off */ + 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; + } + + /* DW1000 deep sleep */ + if (!g_dw1000_sleeping) { + if (idle_ms >= PM_DW1000_SLEEP_MS) { + pm_dw1000_sleep(); } - } 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); + } else { + bool window_expired = g_dw1000_in_window && + (now - g_dw1000_wake_ms) >= PM_DW1000_WAKE_WINDOW_MS; + bool period_elapsed = !g_dw1000_in_window && + (now - g_dw1000_sleep_ms) >= PM_DW1000_WAKE_PERIOD_MS; - if (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); + if (window_expired) { + pm_dw1000_sleep(); + } else if (period_elapsed) { + pm_dw1000_wake(); } } - return range_mm; -} -/* ── Discovery scan ─────────────────────────────────────────────── */ - -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++; + /* GPIO0 hold 3s → 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; + if (!g_pm_display_on) { + display.ssd1306_command(SSD1306_DISPLAYON); + g_pm_display_on = true; + } + } + g_btn_held_ms = 0; } - - 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) ─────────────────────────────────── */ +/* ── OLED display (5 Hz) ────────────────────────────────────────── */ -static uint32_t g_display_last = 0; +static uint32_t g_disp_last = 0; static void display_update(void) { - if (millis() - g_display_last < 200) return; - g_display_last = millis(); + if (!g_pm_display_on) return; + if (millis() - g_disp_last < 200) return; + g_disp_last = millis(); display.clearDisplay(); + uint32_t now = millis(); - /* E-Stop takes full screen */ + /* E-stop override */ if (g_estop_active) { - display.setTextSize(3); - display.setTextColor(SSD1306_WHITE); - display.setCursor(10, 4); - display.println(F("E-STOP")); - display.setTextSize(1); - display.setCursor(20, 48); - display.println(F("RELEASE TO CLEAR")); - display.display(); - return; - } - - /* Discovery scan progress */ - if (g_disc_state == DISC_SCAN) { - display.setTextSize(1); - display.setTextColor(SSD1306_WHITE); - display.setCursor(0, 0); - 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; - } - - 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(0, 0); - display.println(F("NO ANCHOR")); + display.setCursor(10, 0); + display.println(F("!! FALL !!")); 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, 24); + display.printf("Impact: %.1fg", g_imu.accel_mag); 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.println(F("E-STOP ACTIVE")); + display.setCursor(0, 50); + display.printf("Clears in %ds", + (int)((FALL_COOLDOWN_MS - (now - g_fall_last_ms)) / 1000)); display.display(); return; } - /* Normal: find closest active anchor */ - int32_t min_range = INT32_MAX; - 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; + /* DW1000 sleep overlay */ + if (g_dw1000_sleeping && !g_dw1000_in_window) { + uint32_t next_wake_ms = PM_DW1000_WAKE_PERIOD_MS - + (now - g_dw1000_sleep_ms); + display.setTextSize(1); + display.setTextColor(SSD1306_WHITE); + display.setCursor(0, 0); + display.println(F("UWB SLEEP")); + display.setCursor(0, 16); + display.printf("Wake in %ds", (int)(next_wake_ms / 1000)); + display.setCursor(0, 32); + display.printf("Idle: %ds", (int)((now - g_pm_last_activity_ms) / 1000)); + display.setCursor(0, 48); + display.println(F("Hold BTN 3s: off")); + display.display(); + return; } - /* Line 1: big distance + anchor count badge */ - display.setTextSize(3); + /* ── Normal display ── */ + + /* Top-right: connection indicators */ + display.setTextSize(1); display.setTextColor(SSD1306_WHITE); + int x_ind = 128; + if (g_ble_connected) { + x_ind -= 18; + display.setCursor(x_ind, 0); + display.print(F("BT")); + } + bool gps_live = g_gps_last_rx_ms > 0 && (now - g_gps_last_rx_ms) < 3000; + if (gps_live && g_gps.fix_type > 0) { + x_ind -= 24; + display.setCursor(x_ind, 0); + display.printf("G%s", g_gps.fix_type == 2 ? "3D" : "2D"); + } else if (g_ble_connected) { + x_ind -= 24; + display.setCursor(x_ind, 0); + display.print(F("G--")); + } + + /* Line 1: Distance to nearest anchor (big) */ + 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]; + } + + display.setTextSize(3); 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 */ + /* Line 2: Per-anchor ranges */ display.setTextSize(1); display.setCursor(0, 28); - 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 + 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 { display.printf("A%d:--- ", i); + } } - /* Line 3: link status + RSSI bars */ + /* Line 3: GPS speed+heading or IMU */ display.setCursor(0, 40); - 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; - } - } - 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); + if (gps_live && g_gps.fix_type > 0) { + float speed_kmh = g_gps.speed_cmps * 0.036f; /* cm/s → km/h */ + float heading = g_gps.heading_cd / 100.0f; + display.printf("%.0fkm/h %03.0f", speed_kmh, heading); + display.drawCircle(display.getCursorX() + 2, 40, 1, SSD1306_WHITE); /* degree symbol */ + } else 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); } else { - display.println(F("STALE -- searching --")); + 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; + } + } + display.println(any_linked ? F("LINKED") : F("SEARCHING...")); } - /* Line 4: uptime */ + /* Line 4: Uptime + packet count + IMU indicator */ display.setCursor(0, 54); uint32_t secs = now / 1000; - display.printf("UP %02d:%02d seq:%d", secs / 60, secs % 60, g_seq); + display.printf("UP%02d:%02d s:%d", (int)(secs / 60), (int)(secs % 60), g_uwb_seq); + if (g_imu_ok) { + display.setCursor(100, 54); + display.print(F("IMU")); + } 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); + + digitalWrite(PIN_LED, HIGH); + delay(1); + digitalWrite(PIN_LED, LOW); + + pm_activity(); + + /* + * After a successful TWR cycle, transmit any pending data frame. + * The DW1000 is briefly idle between ranging exchanges. + */ + g_ranging_active = 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) { @@ -614,134 +995,153 @@ void setup(void) { pinMode(PIN_LED, OUTPUT); digitalWrite(PIN_LED, LOW); - Serial.printf("\r\n[uwb_tag] tag_id=0x%02X probe_ids=0..%d starting\r\n", - TAG_ID, DISC_PROBE_IDS - 1); + 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 (v2 — UWB data frames)\r\n", TAG_ID); + + /* Build BLE device name from MAC */ + uint8_t mac[6]; + esp_read_mac(mac, ESP_MAC_BT); + 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); /* OLED */ - Wire.begin(PIN_SDA, PIN_SCL); if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { - Serial.println("[uwb_tag] WARN: SSD1306 not found — running headless"); + Serial.println("[uwb_tag] WARN: SSD1306 not found"); } else { display.clearDisplay(); + display.ssd1306_command(SSD1306_DISPLAYON); 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 v2.1", TAG_ID); + display.printf("Tag 0x%02X v2", TAG_ID); display.setCursor(0, 35); - display.println(F("DW3000 DS-TWR + ESP-NOW")); + display.println(F("DW1000+BLE GPS/IMU")); display.setCursor(0, 50); - display.println(F("Scanning anchors...")); + display.printf("BLE: %s", g_ble_device_name); display.display(); - Serial.println("[uwb_tag] OLED ok"); } - /* ESP-NOW */ - WiFi.mode(WIFI_STA); - WiFi.disconnect(); - esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE); - 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"); + cfg_load(); + cfg_apply(); - /* 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); + /* 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"); - 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 (no WiFi needed — ESP-NOW removed) */ + ble_init(); + + /* DW1000 */ + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI); + dw1000_ranging_init(); + + 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; } - dwt_setrxantennadelay(ANT_DELAY); - dwt_settxantennadelay(ANT_DELAY); - dwt_settxpower(0x0E080222UL); + g_pm_last_activity_ms = millis(); - 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); - - 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..."); - - disc_start_scan(); /* begin probing on startup */ + Serial.println("[uwb_tag] DW1000 ready MODE_LONGDATA_RANGE_LOWPOWER"); + Serial.println("[uwb_tag] Ranging + BLE GPS/IMU input + UWB data TX active"); + Serial.printf("[pm] Timeouts: display=%ds DW1000=%ds deep_sleep=hold_%ds\r\n", + g_cfg.sleep_timeout_s, + (int)(PM_DW1000_SLEEP_MS / 1000), + (int)(PM_DEEP_SLEEP_HOLD_MS / 1000)); } -/* ── Main loop ──────────────────────────────────────────────────── */ +/* ── Loop ───────────────────────────────────────────────────────── */ + +static uint32_t g_last_hb = 0; +static uint32_t g_last_imu_read = 0; +static uint32_t g_last_timu_tx = 0; +static uint32_t g_last_gps_tx = 0; +static uint32_t g_last_pimu_tx = 0; void loop(void) { - 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 (now - last_hb_ms >= 1000) { - espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f); - last_hb_ms = now; + /* IMU read at ~50 Hz */ + if (g_imu_ok && (now - g_last_imu_read >= 20)) { + mpu6050_read(&g_imu); + fall_detect(&g_imu); + g_last_imu_read = now; } - /* One ranging slot per RANGE_INTERVAL_MS */ - if (now - last_range_ms >= RANGE_INTERVAL_MS) { - last_range_ms = now; + estop_check(); - 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++; - } + /* DW1000 ranging — skip while sleeping */ + if (!g_dw1000_sleeping && !g_estop_active) { + g_ranging_active = true; + DW1000Ranging.loop(); + /* newRange() callback sets g_ranging_active = false after TWR completes */ + } - /* 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(); - } + /* + * Interleave data frame TX between ranging cycles. + * Only transmit when ranging is not actively using the transceiver. + * Priority: E-stop > GPS > Phone IMU > Tag IMU > Heartbeat + */ + if (!g_dw1000_sleeping && !g_ranging_active) { + + /* GPS relay at 5 Hz */ + if (g_gps_fresh && (now - g_last_gps_tx >= 200)) { + uwb_queue_frame(UWB_MSG_GPS, &g_gps, sizeof(GpsInput)); + uwb_tx_flush(); + g_gps_fresh = false; + g_last_gps_tx = now; + pm_activity(); + } + /* Phone IMU relay at 10 Hz */ + else if (g_pimu_fresh && (now - g_last_pimu_tx >= 100)) { + uwb_queue_frame(UWB_MSG_PIMU, &g_pimu, sizeof(PhoneImuInput)); + uwb_tx_flush(); + g_pimu_fresh = false; + g_last_pimu_tx = now; + } + /* Tag onboard IMU at 10 Hz */ + else if (g_imu_ok && (now - g_last_timu_tx >= 100)) { + TagImuPayload timu = {}; + timu.ax_mg = (int16_t)(g_imu.ax * 1000.0f); + timu.ay_mg = (int16_t)(g_imu.ay * 1000.0f); + timu.az_mg = (int16_t)(g_imu.az * 1000.0f); + timu.gx_dps10 = (int16_t)(g_imu.gx * 10.0f); + timu.gy_dps10 = (int16_t)(g_imu.gy * 10.0f); + timu.gz_dps10 = (int16_t)(g_imu.gz * 10.0f); + timu.accel_mag_10 = (uint8_t)constrain((int)(g_imu.accel_mag * 10.0f), 0, 255); + timu.flags = g_fall_detected ? 0x01 : 0x00; + uwb_queue_frame(UWB_MSG_TIMU, &timu, sizeof(timu)); + uwb_tx_flush(); + g_last_timu_tx = now; + } + /* Heartbeat every 1s */ + else if (now - g_last_hb >= 1000) { + uint8_t hb_payload = 0; /* minimal heartbeat */ + uwb_queue_frame(UWB_MSG_HB, &hb_payload, 1); + uwb_tx_flush(); + g_last_hb = now; } } + /* Power management */ + pm_update(); + + /* Display at 5 Hz */ display_update(); + + delay(1); }