diff --git a/esp32/uwb_tag/platformio.ini b/esp32/uwb_tag/platformio.ini new file mode 100644 index 0000000..590eee0 --- /dev/null +++ b/esp32/uwb_tag/platformio.ini @@ -0,0 +1,27 @@ +; SaltyBot UWB Tag Firmware — Issue #545 +; Target: Makerfabs ESP32 UWB Pro (DW3000 chip) +; +; The tag is battery-powered, worn by the person being tracked. +; It initiates DS-TWR ranging with each anchor in round-robin. +; +; 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 +board = esp32dev +framework = arduino +monitor_speed = 115200 +upload_speed = 921600 +lib_deps = + https://github.com/Makerfabs/MaUWB_DW3000.git +build_flags = + -DCORE_DEBUG_LEVEL=0 + -DTAG_ID=0x01 ; unique per tag (0x01–0xFE) + -DNUM_ANCHORS=2 ; number of anchors to range with + -DRANGE_INTERVAL_MS=50 ; 20 Hz round-robin across anchors diff --git a/esp32/uwb_tag/src/main.cpp b/esp32/uwb_tag/src/main.cpp new file mode 100644 index 0000000..6d79e15 --- /dev/null +++ b/esp32/uwb_tag/src/main.cpp @@ -0,0 +1,363 @@ +/* + * uwb_tag — SaltyBot ESP32 UWB Pro tag firmware (DS-TWR initiator) + * Issue #545 + * + * Hardware: Makerfabs ESP32 UWB Pro (DW3000 chip), battery-powered + * + * Role + * ──── + * Tag is worn by the person being tracked. It initiates DS-TWR + * ranging with each anchor on the robot in round-robin fashion. + * The anchors independently compute and report the range to the + * Jetson Orin (via their own USB serial AT+RANGE output). + * + * Protocol: Double-Sided TWR (DS-TWR) — tag as initiator + * ──────────────────────────────────────────────────────── + * Tag → POLL (msg_type 0x01) to anchor[i] + * Anchor → RESP (msg_type 0x02) with T_poll_rx, T_resp_tx + * Tag → FINAL (msg_type 0x03) with Ra, Da, Db timestamps + * + * The anchor side computes the range and reports it to the Jetson. + * The tag also computes range locally for debug/LED feedback. + * + * Serial output (USB, 115200) — debug only + * ───────────────────────────────────────── + * +RANGE:,,\r\n + * (same format as anchor, useful for bench-testing with USB connected) + * + * Pin mapping — Makerfabs ESP32 UWB Pro + * ────────────────────────────────────── + * SPI SCK 18 SPI MISO 19 SPI MOSI 23 + * DW CS 21 DW RST 27 DW IRQ 34 + * LED 2 (onboard, blinks on each successful range) + * + * Build + * ────── + * pio run -e tag --target upload + * Edit -DTAG_ID= and -DNUM_ANCHORS= in platformio.ini per deployment. + */ + +#include +#include +#include +#include "dw3000.h" // Makerfabs MaUWB_DW3000 library + +/* ── Configurable ───────────────────────────────────────────────── */ + +#ifndef TAG_ID +# define TAG_ID 0x01 /* unique 8-bit address per tag */ +#endif + +#ifndef NUM_ANCHORS +# define NUM_ANCHORS 2 /* anchors to range with (0..N-1) */ +#endif + +#ifndef RANGE_INTERVAL_MS +# define RANGE_INTERVAL_MS 50 /* ms between ranging attempts (20 Hz) */ +#endif + +#define SERIAL_BAUD 115200 +#define PIN_LED 2 + +/* ── Pin map (Makerfabs ESP32 UWB Pro) ─────────────────────────── */ + +#define PIN_SCK 18 +#define PIN_MISO 19 +#define PIN_MOSI 23 +#define PIN_CS 21 +#define PIN_RST 27 +#define PIN_IRQ 34 + +/* ── DW3000 PHY config (must match anchor) ──────────────────────── */ + +static dwt_config_t dw_cfg = { + 5, /* channel 5 */ + DWT_PLEN_128, + DWT_PAC8, + 9, /* TX preamble code */ + 9, /* 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 (shared with anchor firmware) ─────────────────── */ + +#define FTYPE_POLL 0x01 +#define FTYPE_RESP 0x02 +#define FTYPE_FINAL 0x03 + +#define FRAME_HDR 3 +#define FCS_LEN 2 + +/* POLL: header only */ +#define POLL_FRAME_LEN (FRAME_HDR + FCS_LEN) + +/* RESP: T_poll_rx(5) + T_resp_tx(5) */ +#define RESP_PAYLOAD 10 +#define RESP_FRAME_LEN (FRAME_HDR + RESP_PAYLOAD + FCS_LEN) + +/* FINAL: Ra(5) + Da(5) + Db(5) */ +#define FINAL_PAYLOAD 15 +#define FINAL_FRAME_LEN (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN) + +/* ── Timing ────────────────────────────────────────────────────── */ + +/* Tag TX turnaround from resp_rx to final_tx: 500 µs */ +#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) + +/* Timeout waiting for RESP after POLL */ +#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; +} + +/* ── DS-TWR initiator (one anchor, one cycle) ───────────────────── */ + +/* + * Returns range in mm, or -1 on failure. + * + * DS-TWR tag side: + * 1. TX POLL to anchor_id + * 2. RX RESP from anchor → extract T_poll_rx_a, T_resp_tx_a + * Record: T_poll_tx, T_resp_rx + * 3. Compute: + * Ra = T_resp_rx − T_poll_tx (tag round-trip) + * Da = T_final_tx − T_resp_rx (tag turnaround) + * Db = T_resp_tx_a − T_poll_rx_a (anchor turnaround, from RESP) + * 4. TX FINAL with Ra, Da, Db — anchor uses these to compute range + * 5. (Optional) compute range locally from RESP timestamps for debug + */ +static int32_t twr_range_once(uint8_t anchor_id) { + + /* --- 1. TX POLL --- */ + uint8_t poll[POLL_FRAME_LEN]; + poll[0] = FTYPE_POLL; + poll[1] = TAG_ID; + poll[2] = anchor_id; + + dwt_writetxdata(POLL_FRAME_LEN - FCS_LEN, poll, 0); + dwt_writetxfctrl(POLL_FRAME_LEN, 0, 1 /*ranging*/); + + dwt_setrxaftertxdelay(300); + dwt_setrxtimeout(RESP_RX_TIMEOUT_US); + + 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; + + /* Wait for TX done */ + uint32_t t0 = millis(); + while (!g_tx_done && millis() - t0 < 15) yield(); + + /* Read T_poll_tx */ + uint8_t poll_tx_raw[5]; + dwt_readtxtimestamp(poll_tx_raw); + uint64_t T_poll_tx = ts_read(poll_tx_raw); + + /* --- 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; /* dst must be us */ + if (g_rx_buf[1] != anchor_id) return -1; /* src must be target anchor */ + + /* Read T_resp_rx */ + uint8_t resp_rx_raw[5]; + dwt_readrxtimestamp(resp_rx_raw); + uint64_t T_resp_rx = ts_read(resp_rx_raw); + + /* Extract anchor-side timestamps from RESP payload */ + uint64_t T_poll_rx_a = ts_read(&g_rx_buf[3]); /* anchor received poll */ + uint64_t T_resp_tx_a = ts_read(&g_rx_buf[8]); /* anchor sent response */ + + /* --- 3. Compute DS-TWR values to embed in FINAL --- */ + uint64_t Ra = ts_diff(T_resp_rx, T_poll_tx); /* tag round-trip */ + uint64_t Db = ts_diff(T_resp_tx_a, T_poll_rx_a); /* anchor turnaround*/ + + /* Compute T_final_tx: resp_rx + turnaround, aligned to 512-tick grid */ + uint64_t final_tx_sched = (T_resp_rx + FINAL_TX_DLY_TICKS) & ~0x1FFULL; + uint64_t Da = ts_diff(final_tx_sched, T_resp_rx); /* tag turnaround */ + + /* --- 4. TX FINAL with Ra, Da, Db --- */ + 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 /*ranging*/); + dwt_setdelayedtrxtime((uint32_t)(final_tx_sched >> 8)); + + g_tx_done = false; + int tx_ret = dwt_starttx(DWT_START_TX_DELAYED); + if (tx_ret != DWT_SUCCESS) { + dwt_forcetrxoff(); + return -1; + } + t0 = millis(); + while (!g_tx_done && millis() - t0 < 15) yield(); + + /* --- 5. Local range computation (debug) --- */ + /* Read actual T_final_tx */ + uint8_t final_tx_raw[5]; + dwt_readtxtimestamp(final_tx_raw); + uint64_t T_final_tx = ts_read(final_tx_raw); + uint64_t Da_actual = ts_diff(T_final_tx, T_resp_rx); + + /* Single-sided estimate from tag's perspective (anchor will do DS-TWR) */ + double ra = ticks_to_s(Ra); + double da = ticks_to_s(Da_actual); + double db = ticks_to_s(Db); + /* For local display use simplified estimate: tof ≈ (Ra - Db) / 2 */ + 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); +} + +/* ── Setup ──────────────────────────────────────────────────────── */ + +void setup(void) { + Serial.begin(SERIAL_BAUD); + delay(300); + + pinMode(PIN_LED, OUTPUT); + digitalWrite(PIN_LED, LOW); + + Serial.printf("\r\n[uwb_tag] tag_id=0x%02X num_anchors=%d starting\r\n", + TAG_ID, NUM_ANCHORS); + + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + + /* Hardware reset */ + 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 — check SPI wiring"); + 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); + } + + dwt_setrxantennadelay(ANT_DELAY); + dwt_settxantennadelay(ANT_DELAY); + dwt_settxpower(0x0E080222UL); + + 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_id=0x%02X\r\n", + dw_cfg.chan, TAG_ID); + Serial.println("[uwb_tag] Starting ranging..."); +} + +/* ── Main loop — round-robin across anchors ─────────────────────── */ + +void loop(void) { + static uint8_t anchor_idx = 0; + static uint32_t last_range_ms = 0; + + uint32_t now = millis(); + if (now - last_range_ms < RANGE_INTERVAL_MS) { + yield(); + return; + } + last_range_ms = now; + + uint8_t anchor_id = anchor_idx % NUM_ANCHORS; + int32_t range_mm = twr_range_once(anchor_id); + + if (range_mm > 0) { + float rssi = rx_power_dbm(); + Serial.printf("+RANGE:%d,%ld,%.1f\r\n", anchor_id, (long)range_mm, rssi); + /* Brief LED blink */ + digitalWrite(PIN_LED, HIGH); + delay(2); + digitalWrite(PIN_LED, LOW); + } + + anchor_idx++; + if (anchor_idx >= NUM_ANCHORS) anchor_idx = 0; +}