feat: ESP32 UWB Pro anchor firmware — DS-TWR responder (Issue #544)
Anchor firmware for Makerfabs ESP32 UWB Pro (DW3000 chip). Two anchors mount on SaltyBot (port/starboard), USB-connected to Jetson Orin. - DS-TWR responder: Poll→Resp→Final with ±10cm accuracy - Streams +RANGE:<id>,<mm>,<rssi_dbm> on Serial 115200 - AT command interface: AT+RANGE?, AT+RANGE_ADDR=, AT+ID? - ANCHOR_ID 0/1 set at build time (env:anchor0 / env:anchor1) - PlatformIO config for Makerfabs MaUWB_DW3000 library - udev rules for /dev/uwb-anchor0 /dev/uwb-anchor1 USB symlinks - Pin map: SCK=18 MISO=19 MOSI=23 CS=21 RST=27 IRQ=34 Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-uwb
parent
76668d8346
commit
a4879b6b3f
30
esp32/uwb_anchor/platformio.ini
Normal file
30
esp32/uwb_anchor/platformio.ini
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
; SaltyBot UWB Anchor Firmware — Issue #544
|
||||||
|
; Target: Makerfabs ESP32 UWB Pro (DW3000 chip)
|
||||||
|
;
|
||||||
|
; Library: Makerfabs MaUWB_DW3000
|
||||||
|
; https://github.com/Makerfabs/MaUWB_DW3000
|
||||||
|
;
|
||||||
|
; Flash:
|
||||||
|
; pio run -e anchor0 --target upload (port-side anchor)
|
||||||
|
; pio run -e anchor1 --target upload (starboard anchor)
|
||||||
|
; Monitor:
|
||||||
|
; pio device monitor -e anchor0 -b 115200
|
||||||
|
|
||||||
|
[common]
|
||||||
|
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
|
||||||
|
|
||||||
|
[env:anchor0]
|
||||||
|
extends = common
|
||||||
|
build_flags = ${common.build_flags} -DANCHOR_ID=0
|
||||||
|
|
||||||
|
[env:anchor1]
|
||||||
|
extends = common
|
||||||
|
build_flags = ${common.build_flags} -DANCHOR_ID=1
|
||||||
413
esp32/uwb_anchor/src/main.cpp
Normal file
413
esp32/uwb_anchor/src/main.cpp
Normal file
@ -0,0 +1,413 @@
|
|||||||
|
/*
|
||||||
|
* 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:<anchor_id>,<range_mm>,<rssi_dbm>\r\n (on each successful range)
|
||||||
|
*
|
||||||
|
* AT commands (host → anchor)
|
||||||
|
* ───────────────────────────
|
||||||
|
* AT+RANGE? → returns last buffered +RANGE line
|
||||||
|
* AT+RANGE_ADDR=<hex_addr> → pair with specific tag (filter others)
|
||||||
|
* AT+RANGE_ADDR= → clear pairing (accept all tags)
|
||||||
|
* AT+ID? → returns +ID:<anchor_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 <Arduino.h>
|
||||||
|
#include <SPI.h>
|
||||||
|
#include <math.h>
|
||||||
|
#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();
|
||||||
|
}
|
||||||
19
esp32/uwb_anchor/udev/99-uwb-anchors.rules
Normal file
19
esp32/uwb_anchor/udev/99-uwb-anchors.rules
Normal file
@ -0,0 +1,19 @@
|
|||||||
|
# SaltyBot UWB anchor USB-serial persistent symlinks
|
||||||
|
# Install:
|
||||||
|
# sudo cp 99-uwb-anchors.rules /etc/udev/rules.d/
|
||||||
|
# sudo udevadm control --reload && sudo udevadm trigger
|
||||||
|
#
|
||||||
|
# Find serial numbers:
|
||||||
|
# udevadm info -a /dev/ttyUSB0 | grep ATTRS{serial}
|
||||||
|
#
|
||||||
|
# Fill ANCHOR0_SERIAL and ANCHOR1_SERIAL with the values found above.
|
||||||
|
# Anchor 0 = port side → /dev/uwb-anchor0
|
||||||
|
# Anchor 1 = starboard → /dev/uwb-anchor1
|
||||||
|
|
||||||
|
SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
|
||||||
|
ATTRS{serial}=="ANCHOR0_SERIAL", \
|
||||||
|
SYMLINK+="uwb-anchor0", MODE="0666"
|
||||||
|
|
||||||
|
SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
|
||||||
|
ATTRS{serial}=="ANCHOR1_SERIAL", \
|
||||||
|
SYMLINK+="uwb-anchor1", MODE="0666"
|
||||||
Loading…
x
Reference in New Issue
Block a user