/* * uwb_anchor — SaltyBot ESP32 UWB Pro anchor firmware (TWR responder) * Issue #544 * * Hardware: Makerfabs ESP32 UWB Pro (DW3000 chip) * * Role * ──── * Anchor sits on SaltyBot body, USB-connected to Jetson Orin. * Two anchors per robot (anchor-0 port side, anchor-1 starboard). * Person-worn tags initiate ranging; anchors respond. * * Protocol: Double-Sided TWR (DS-TWR) * ──────────────────────────────────── * Tag → Anchor POLL (msg_type 0x01) * Anchor → Tag RESP (msg_type 0x02, payload: T_poll_rx, T_resp_tx) * Tag → Anchor FINAL (msg_type 0x03, payload: Ra, Da, Db timestamps) * Anchor computes range via DS-TWR formula, emits +RANGE on Serial. * * Serial output (115200 8N1, USB-CDC to Jetson) * ────────────────────────────────────────────── * +RANGE:,,\r\n (on each successful range) * * AT commands (host → anchor) * ─────────────────────────── * AT+RANGE? → returns last buffered +RANGE line * AT+RANGE_ADDR= → pair with specific tag (filter others) * AT+RANGE_ADDR= → clear pairing (accept all tags) * AT+ID? → returns +ID: * * Pin mapping — Makerfabs ESP32 UWB Pro * ────────────────────────────────────── * SPI SCK 18 SPI MISO 19 SPI MOSI 23 * DW CS 21 DW RST 27 DW IRQ 34 * * Build * ────── * pio run -e anchor0 --target upload (port side) * pio run -e anchor1 --target upload (starboard) */ #include #include #include #include "dw3000.h" // Makerfabs MaUWB_DW3000 library /* ── Configurable ───────────────────────────────────────────────── */ #ifndef ANCHOR_ID # define ANCHOR_ID 0 /* 0 = port, 1 = starboard */ #endif #define SERIAL_BAUD 115200 /* ── 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 channel / PHY config ───────────────────────────────── */ static dwt_config_t dw_cfg = { 5, /* channel 5 (6.5 GHz, best penetration) */ DWT_PLEN_128, /* preamble length */ DWT_PAC8, /* PAC size */ 9, /* TX preamble code */ 9, /* RX preamble code */ 1, /* SFD type (IEEE 802.15.4z) */ DWT_BR_6M8, /* data rate 6.8 Mbps */ DWT_PHR_MODE_STD, /* standard PHR */ DWT_PHR_RATE_DATA, (129 + 8 - 8), /* SFD timeout */ DWT_STS_MODE_OFF, /* STS off — standard TWR */ DWT_STS_LEN_64, DWT_PDOA_M0, /* no PDoA */ }; /* ── Frame format ──────────────────────────────────────────────── */ /* Byte layout for all frames: * [0] frame_type (FTYPE_*) * [1] src_id (tag 8-bit addr, or ANCHOR_ID) * [2] dst_id * [3..] payload * (FCS appended automatically by DW3000 — 2 bytes) */ #define FTYPE_POLL 0x01 #define FTYPE_RESP 0x02 #define FTYPE_FINAL 0x03 #define FRAME_HDR 3 #define FCS_LEN 2 /* RESP payload: T_poll_rx(5 B) + T_resp_tx(5 B) */ #define RESP_PAYLOAD 10 #define RESP_FRAME_LEN (FRAME_HDR + RESP_PAYLOAD + FCS_LEN) /* FINAL payload: Ra(5 B) + Da(5 B) + Db(5 B) */ #define FINAL_PAYLOAD 15 #define FINAL_FRAME_LEN (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN) /* ── Timing ────────────────────────────────────────────────────── */ /* Turnaround delay: anchor waits 500 µs after poll_rx before tx_resp. * DW3000 tick = 1/(128×499.2e6) ≈ 15.65 ps → 500 µs = ~31.95M ticks. * Stored as uint32 shifted right 8 bits for dwt_setdelayedtrxtime. */ #define RESP_TX_DLY_US 500UL #define DWT_TICKS_PER_US 63898UL /* 1µs in DW3000 ticks (×8 prescaler) */ #define RESP_TX_DLY_TICKS (RESP_TX_DLY_US * DWT_TICKS_PER_US) /* How long anchor listens for FINAL after sending RESP */ #define FINAL_RX_TIMEOUT_US 3000 /* Speed of light (m/s) */ #define SPEED_OF_LIGHT 299702547.0 /* DW3000 40-bit timestamp mask */ #define DWT_TS_MASK 0xFFFFFFFFFFULL /* Antenna delay (factory default; calibrate per unit for best accuracy) */ #define ANT_DELAY 16385 /* ── Interrupt flags (set in ISR, polled in main) ──────────────── */ 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; /* ── State ──────────────────────────────────────────────────────── */ /* Last successful range (serves AT+RANGE? queries) */ static int32_t g_last_range_mm = -1; static char g_last_range_line[72] = {}; /* Optional tag pairing: 0 = accept all tags */ static uint8_t g_paired_tag_id = 0; /* ── DW3000 ISR callbacks ───────────────────────────────────────── */ 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); } /* Estimate receive power from CIR diagnostics (dBm) */ 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; } /* ── AT command handler ─────────────────────────────────────────── */ static char g_at_buf[64]; static int g_at_idx = 0; static void at_dispatch(const char *cmd) { if (strcmp(cmd, "AT+RANGE?") == 0) { if (g_last_range_mm >= 0) Serial.println(g_last_range_line); else Serial.println("+RANGE:NO_DATA"); } else if (strcmp(cmd, "AT+ID?") == 0) { Serial.printf("+ID:%d\r\n", ANCHOR_ID); } else if (strncmp(cmd, "AT+RANGE_ADDR=", 14) == 0) { const char *v = cmd + 14; if (*v == '\0') { g_paired_tag_id = 0; Serial.println("+OK:UNPAIRED"); } else { g_paired_tag_id = (uint8_t)strtoul(v, nullptr, 0); Serial.printf("+OK:PAIRED=0x%02X\r\n", g_paired_tag_id); } } else { Serial.println("+ERR:UNKNOWN_CMD"); } } static void serial_poll(void) { while (Serial.available()) { char c = (char)Serial.read(); if (c == '\r') continue; if (c == '\n') { g_at_buf[g_at_idx] = '\0'; if (g_at_idx > 0) at_dispatch(g_at_buf); g_at_idx = 0; } else if (g_at_idx < (int)(sizeof(g_at_buf) - 1)) { g_at_buf[g_at_idx++] = c; } } } /* ── DS-TWR anchor state machine ────────────────────────────────── */ /* * DS-TWR responder (one shot): * 1. Wait for POLL from tag * 2. Delayed-TX RESP (carry T_poll_rx + scheduled T_resp_tx) * 3. Wait for FINAL from tag (tag embeds Ra, Da, Db) * 4. Compute: Rb = T_final_rx − T_resp_tx * tof = (Ra·Rb − Da·Db) / (Ra+Rb+Da+Db) * range_m = tof × c * 5. Print +RANGE line */ static void twr_cycle(void) { /* --- 1. Listen for POLL --- */ dwt_setrxtimeout(0); dwt_rxenable(DWT_START_RX_IMMEDIATE); g_rx_ok = g_rx_err = false; uint32_t deadline = millis() + 2000; while (!g_rx_ok && !g_rx_err) { serial_poll(); if (millis() > deadline) { /* restart RX if we've been stuck */ dwt_rxenable(DWT_START_RX_IMMEDIATE); deadline = millis() + 2000; } yield(); } if (!g_rx_ok || g_rx_len < FRAME_HDR) return; /* validate POLL */ if (g_rx_buf[0] != FTYPE_POLL) return; uint8_t tag_id = g_rx_buf[1]; if (g_paired_tag_id != 0 && tag_id != g_paired_tag_id) return; /* --- 2. Record T_poll_rx --- */ uint8_t poll_rx_raw[5]; dwt_readrxtimestamp(poll_rx_raw); uint64_t T_poll_rx = ts_read(poll_rx_raw); /* Compute delayed TX time: poll_rx + turnaround, aligned to 512-tick grid */ uint64_t resp_tx_sched = (T_poll_rx + RESP_TX_DLY_TICKS) & ~0x1FFULL; /* Build RESP frame */ uint8_t resp[RESP_FRAME_LEN]; resp[0] = FTYPE_RESP; resp[1] = ANCHOR_ID; resp[2] = tag_id; ts_write(&resp[3], T_poll_rx); /* T_poll_rx (tag uses this) */ ts_write(&resp[8], resp_tx_sched); /* scheduled T_resp_tx */ dwt_writetxdata(RESP_FRAME_LEN - FCS_LEN, resp, 0); dwt_writetxfctrl(RESP_FRAME_LEN, 0, 1 /*ranging*/); dwt_setdelayedtrxtime((uint32_t)(resp_tx_sched >> 8)); /* Enable RX after TX to receive FINAL */ dwt_setrxaftertxdelay(300); dwt_setrxtimeout(FINAL_RX_TIMEOUT_US); /* Fire delayed TX */ g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false; if (dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) { dwt_forcetrxoff(); return; /* TX window missed — try next cycle */ } /* Wait for TX done (short wait, ISR fires fast) */ uint32_t t0 = millis(); while (!g_tx_done && millis() - t0 < 15) { yield(); } /* Read actual T_resp_tx */ uint8_t resp_tx_raw[5]; dwt_readtxtimestamp(resp_tx_raw); uint64_t T_resp_tx = ts_read(resp_tx_raw); /* --- 3. Wait for FINAL --- */ t0 = millis(); while (!g_rx_ok && !g_rx_err && !g_rx_to && millis() - t0 < 60) { serial_poll(); yield(); } if (!g_rx_ok || g_rx_len < FRAME_HDR + FINAL_PAYLOAD) return; if (g_rx_buf[0] != FTYPE_FINAL) return; if (g_rx_buf[1] != tag_id) return; /* Extract DS-TWR timestamps from FINAL payload */ uint64_t Ra = ts_read(&g_rx_buf[3]); /* tag: T_resp_rx − T_poll_tx */ uint64_t Da = ts_read(&g_rx_buf[8]); /* tag: T_final_tx − T_resp_rx */ /* g_rx_buf[13..17] = Db from tag (cross-check, unused here) */ /* T_final_rx */ uint8_t final_rx_raw[5]; dwt_readrxtimestamp(final_rx_raw); uint64_t T_final_rx = ts_read(final_rx_raw); /* --- 4. DS-TWR formula --- */ uint64_t Rb = ts_diff(T_final_rx, T_resp_tx); /* anchor round-trip */ uint64_t Db = ts_diff(T_resp_tx, T_poll_rx); /* anchor turnaround */ double ra = ticks_to_s(Ra), rb = ticks_to_s(Rb); double da = ticks_to_s(Da), db = ticks_to_s(Db); double denom = ra + rb + da + db; if (denom < 1e-15) return; double tof = (ra * rb - da * db) / denom; double range_m = tof * SPEED_OF_LIGHT; /* Validity window: 0.1 m – 130 m */ if (range_m < 0.1 || range_m > 130.0) return; int32_t range_mm = (int32_t)(range_m * 1000.0 + 0.5); float rssi = rx_power_dbm(); /* --- 5. Emit +RANGE --- */ snprintf(g_last_range_line, sizeof(g_last_range_line), "+RANGE:%d,%ld,%.1f", ANCHOR_ID, (long)range_mm, rssi); g_last_range_mm = range_mm; Serial.println(g_last_range_line); } /* ── Arduino setup ──────────────────────────────────────────────── */ void setup(void) { Serial.begin(SERIAL_BAUD); delay(300); Serial.printf("\r\n[uwb_anchor] anchor_id=%d starting\r\n", ANCHOR_ID); 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); /* DW3000 probe + init (Makerfabs MaUWB_DW3000 library) */ if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) { Serial.println("[uwb_anchor] FATAL: DW3000 probe failed — check SPI wiring"); for (;;) delay(1000); } if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) { Serial.println("[uwb_anchor] FATAL: dwt_initialise failed"); for (;;) delay(1000); } if (dwt_configure(&dw_cfg) != DWT_SUCCESS) { Serial.println("[uwb_anchor] FATAL: dwt_configure failed"); for (;;) delay(1000); } dwt_setrxantennadelay(ANT_DELAY); dwt_settxantennadelay(ANT_DELAY); dwt_settxpower(0x0E080222UL); /* max TX power for 120 m range */ 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_anchor] DW3000 ready ch=%d 6.8Mbps id=%d\r\n", dw_cfg.chan, ANCHOR_ID); Serial.println("[uwb_anchor] Listening for tags..."); } /* ── Arduino loop ───────────────────────────────────────────────── */ void loop(void) { serial_poll(); twr_cycle(); }