feat: ROS2 sensor health monitor (Issue #566) #571
27
esp32/uwb_tag/platformio.ini
Normal file
27
esp32/uwb_tag/platformio.ini
Normal file
@ -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
|
||||||
363
esp32/uwb_tag/src/main.cpp
Normal file
363
esp32/uwb_tag/src/main.cpp
Normal file
@ -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:<anchor_id>,<range_mm>,<rssi_dbm>\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 <Arduino.h>
|
||||||
|
#include <SPI.h>
|
||||||
|
#include <math.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
Loading…
x
Reference in New Issue
Block a user