feat: UWB tag firmware (Issue #545) #568

sl-jetson · 2026-03-14T11:43:48-04:00

sl-jetson commented

2026-03-14 11:43:48 -04:00

Summary

esp32/uwb_tag/uwb_tag.ino: SS-TWR initiator firmware for Makerfabs ESP32 UWB Pro (DW3000)
esp32/uwb_tag/config.h: user-editable config — anchor addresses, PAN ID, tag address, poll rate, pins, antenna delays

Protocol: Single-Sided TWR with tag as initiator

Tag sends POLL (records T_sp on TX done)
Anchor sends RESP embedding T_rp and T_sr in payload
Tag receives RESP (records T_rr), computes ToF = (Round − Reply) / 2, outputs JSON

Serial output (115200 8N1) — one JSON line per event:

{"anchor_id":0,"range_m":1.234,"rssi":-78.5,"seq":42,"ts_ms":5678}
{"anchor_id":1,"event":"timeout","seq":43,"ts_ms":5698}
{"anchor_id":0,"event":"anchor_lost","ts_ms":7800}
{"event":"ready","tag_addr":1,"num_anchors":2,"poll_hz":10}

Key features:

10 Hz per-anchor poll rate; anchors polled in sequence with even spacing
Anchor-lost watchdog: anchor_lost event after 2 s of silence
IEEE 802.15.4a short-address framing with DW3000 HW frame filter
RSSI from DW3000 CIA first-path amplitude (Decawave APS006 formula)
Boot error reported as JSON; loops forever on DW3000 init failure

Deps: Makerfabs-ESP32-UWB (dw3000.h), ArduinoJson ≥ v6

Test plan

Flash to ESP32 UWB Pro, open serial at 115200 — see {"event":"ready",...}
With anchor firmware running on 2× DW3000 anchors (addr 0x0010, 0x0011): verify range JSON at ~10 Hz per anchor
Disconnect one anchor > 2 s — verify anchor_lost event fires
Reconnect anchor — verify ranging resumes, watchdog clears
Check range_m against tape measure at known 1 m / 3 m / 5 m distances

🤖 Generated with Claude Code

## Summary - `esp32/uwb_tag/uwb_tag.ino`: SS-TWR initiator firmware for **Makerfabs ESP32 UWB Pro** (DW3000) - `esp32/uwb_tag/config.h`: user-editable config — anchor addresses, PAN ID, tag address, poll rate, pins, antenna delays **Protocol:** Single-Sided TWR with tag as initiator - Tag sends POLL (records `T_sp` on TX done) - Anchor sends RESP embedding `T_rp` and `T_sr` in payload - Tag receives RESP (records `T_rr`), computes `ToF = (Round − Reply) / 2`, outputs JSON **Serial output (115200 8N1) — one JSON line per event:** ``` {"anchor_id":0,"range_m":1.234,"rssi":-78.5,"seq":42,"ts_ms":5678} {"anchor_id":1,"event":"timeout","seq":43,"ts_ms":5698} {"anchor_id":0,"event":"anchor_lost","ts_ms":7800} {"event":"ready","tag_addr":1,"num_anchors":2,"poll_hz":10} ``` **Key features:** - 10 Hz per-anchor poll rate; anchors polled in sequence with even spacing - Anchor-lost watchdog: `anchor_lost` event after 2 s of silence - IEEE 802.15.4a short-address framing with DW3000 HW frame filter - RSSI from DW3000 CIA first-path amplitude (Decawave APS006 formula) - Boot error reported as JSON; loops forever on DW3000 init failure **Deps:** `Makerfabs-ESP32-UWB` (dw3000.h), `ArduinoJson` ≥ v6 ## Test plan - [ ] Flash to ESP32 UWB Pro, open serial at 115200 — see `{"event":"ready",...}` - [ ] With anchor firmware running on 2× DW3000 anchors (addr 0x0010, 0x0011): verify range JSON at ~10 Hz per anchor - [ ] Disconnect one anchor > 2 s — verify `anchor_lost` event fires - [ ] Reconnect anchor — verify ranging resumes, watchdog clears - [ ] Check `range_m` against tape measure at known 1 m / 3 m / 5 m distances 🤖 Generated with [Claude Code](https://claude.com/claude-code)

sl-jetson added 1 commit 2026-03-14 11:43:48 -04:00

feat: ROS2 UWB position node (Issue #546 ) 2180b61440

Add saltybot_uwb_position — ROS2 Python package that reads JSON range
measurements from an ESP32 DW3000 UWB tag over USB serial, trilaterates
the robot's absolute position from 3+ fixed infrastructure anchors, and
publishes position + TF2 to the rest of the stack.

Serial protocol (one JSON line per frame):
  Full frame: {"ts":…, "ranges": [{"id":0,"d_mm":1500,"rssi":-65}, …]}
  Per-anchor: {"id":0, "d_mm":1500, "rssi":-65.0}
  Accepts both "d_mm" and "range_mm" field names.

Trilateration (trilateration.py, numpy, no ROS deps):
  Linear least-squares: linearise sphere equations around anchor 0,
  solve (N-1)x2 (2D) or (N-1)x3 (3D) system via np.linalg.lstsq.
  2D mode (default): robot_z fixed, needs >=3 anchors.
  3D mode (solve_z=true): full 3D, needs >=4 anchors.

Outlier rejection:
  After initial solve, compute per-anchor residual |r_meas - r_pred|.
  Reject anchors with residual > outlier_threshold_m (0.4 m default).
  Re-solve with inliers if >= min_anchors remain.
  Track consecutive outlier strikes; flag in /status after N strikes.

Kalman filter (KalmanFilter3D, constant-velocity, 6-state, numpy):
  Predict-only coasting when anchors drop below minimum.
  Q=0.05, R=0.10 (tunable).

Topics:
  /saltybot/uwb/pose       PoseStamped  10 Hz Kalman-filtered position
  /saltybot/uwb/range/<id> UwbRange     on arrival, raw per-anchor ranges
  /saltybot/uwb/status     String/JSON  10 Hz state+residuals+flags

TF2: uwb_link -> map (identity rotation)

Anchor config: flat float arrays in YAML.
Default layout: 4-anchor 5x5m room at 2m height.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

sl-jetson merged commit 3b0b9d0f16 into main

2026-03-14 11:49:44 -04:00

sl-jetson referenced this issue from a commit

2026-03-14 11:49:46 -04:00

Merge pull request 'feat: UWB tag firmware (Issue #545)' (#568) from sl-perception/issue-546-uwb-ros2 into main

Sign in to join this conversation.

1 Participants

Notifications

Due Date

No due date set.

Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#568