seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat: UWB anchor auto-calibration via inter-anchor ranging + MDS (Issue #602) #608

Merged
sl-jetson merged 3 commits from sl-uwb/issue-602-anchor-calibration into main 2026-03-14 15:54:57 -04:00
Conversation 0 Commits 3 Files Changed 21 +1518 -2
sl-jetson commented 2026-03-14 15:06:54 -04:00
Collaborator
Copy Link

Summary

Implements automated UWB anchor position calibration (Issue #602).

Anchor firmware (esp32/uwb_anchor/src/main.cpp)

  • peer_range_once(peer_id) — DS-TWR initiator role toward a peer anchor: POLL → RESP → FINAL exchange with range estimate Ra − Da/2
  • AT+PEER_RANGE=<id> command: triggers inter-anchor ranging, responds with +PEER_RANGE:<my_id>,<peer_id>,<mm>,<rssi> or +PEER_RANGE:ERR,<peer_id>,TIMEOUT

ROS2 saltybot_uwb_calibration_msgs (new package)

  • CalibrateAnchors.srv — request (anchor_ids[], n_samples), response (positions_x/y/z[], residual_rms_m, anchor_positions_json)

ROS2 saltybot_uwb_calibration (new package)

  • mds_math.py — classical MDS: double-centred D², eigendecomposition to recover 2-D positions; anchor_frame_align() fixes anchor-0 at origin, anchor-1 on +X axis
  • calibration_node.py — /saltybot/uwb/calibrate_anchors service: opens anchor serial ports, round-robins AT+PEER_RANGE= for all N×(N-1)/2 pairs, builds distance matrix, runs MDS, returns JSON anchor config
  • Supports ≥ 4 anchors; 5× averaged ranging per pair by default

Tests

  • 12/12 unit tests passing (test/test_mds_math.py) — no ROS2 or hardware required
    • Square and 5-anchor configurations, 5 cm noise tolerance, error handling, frame alignment

Test plan

  • Flash anchor firmware and verify AT+PEER_RANGE=1 returns +PEER_RANGE:0,1,<mm>,<rssi> from anchor 0 toward anchor 1
  • Verify peer anchor also emits its own +RANGE line when initiator's FINAL is received (responder mode stays active)
  • Deploy 4+ anchors, run ros2 launch saltybot_uwb_calibration uwb_calibration.launch.py
  • Call ros2 service call /saltybot/uwb/calibrate_anchors saltybot_uwb_calibration_msgs/srv/CalibrateAnchors "{anchor_ids: [], n_samples: 5}"
  • Confirm residual RMS < 5 cm, JSON positions match physical layout

🤖 Generated with Claude Code

## Summary Implements automated UWB anchor position calibration (Issue #602). ### Anchor firmware (`esp32/uwb_anchor/src/main.cpp`) - `peer_range_once(peer_id)` — DS-TWR **initiator** role toward a peer anchor: POLL → RESP → FINAL exchange with range estimate Ra − Da/2 - `AT+PEER_RANGE=<id>` command: triggers inter-anchor ranging, responds with `+PEER_RANGE:<my_id>,<peer_id>,<mm>,<rssi>` or `+PEER_RANGE:ERR,<peer_id>,TIMEOUT` ### ROS2 `saltybot_uwb_calibration_msgs` (new package) - `CalibrateAnchors.srv` — request `(anchor_ids[], n_samples)`, response `(positions_x/y/z[], residual_rms_m, anchor_positions_json)` ### ROS2 `saltybot_uwb_calibration` (new package) - `mds_math.py` — classical MDS: double-centred D², eigendecomposition to recover 2-D positions; `anchor_frame_align()` fixes anchor-0 at origin, anchor-1 on +X axis - `calibration_node.py` — `/saltybot/uwb/calibrate_anchors` service: opens anchor serial ports, round-robins `AT+PEER_RANGE=` for all N×(N-1)/2 pairs, builds distance matrix, runs MDS, returns JSON anchor config - Supports ≥ 4 anchors; 5× averaged ranging per pair by default ### Tests - 12/12 unit tests passing (`test/test_mds_math.py`) — no ROS2 or hardware required - Square and 5-anchor configurations, 5 cm noise tolerance, error handling, frame alignment ## Test plan - [ ] Flash anchor firmware and verify `AT+PEER_RANGE=1` returns `+PEER_RANGE:0,1,<mm>,<rssi>` from anchor 0 toward anchor 1 - [ ] Verify peer anchor also emits its own `+RANGE` line when initiator's FINAL is received (responder mode stays active) - [ ] Deploy 4+ anchors, run `ros2 launch saltybot_uwb_calibration uwb_calibration.launch.py` - [ ] Call `ros2 service call /saltybot/uwb/calibrate_anchors saltybot_uwb_calibration_msgs/srv/CalibrateAnchors "{anchor_ids: [], n_samples: 5}"` - [ ] Confirm residual RMS < 5 cm, JSON positions match physical layout 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-jetson added 3 commits 2026-03-14 15:06:54 -04:00 
Anchor firmware (esp32/uwb_anchor/src/main.cpp):
- Add peer_range_once(peer_id) — DS-TWR initiator role toward a peer anchor
- Add AT+PEER_RANGE=<id> command: triggers inter-anchor ranging and returns
  +PEER_RANGE:<my_id>,<peer_id>,<range_mm>,<rssi_dbm> (or ERR,TIMEOUT)

ROS2 package saltybot_uwb_calibration_msgs:
- CalibrateAnchors.srv: request (anchor_ids[], n_samples) →
  response (positions_x/y/z[], residual_rms_m, anchor_positions_json)

ROS2 package saltybot_uwb_calibration:
- mds_math.py: classical MDS (double-centred D², eigendecomposition),
  anchor_frame_align() to fix anchor-0 at origin / anchor-1 on +X
- calibration_node.py: /saltybot/uwb/calibrate_anchors service —
  opens anchor serial ports, rounds-robin AT+PEER_RANGE= for all pairs,
  builds N×N distance matrix, runs MDS, returns JSON anchor positions
- 12/12 unit tests passing (test/test_mds_math.py)
- Supports ≥ 4 anchors; 5× averaged ranging per pair by default

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Adds a slow-adapting terrain slope estimator (IIR tau=5s) that decouples
the robot's balance offset from genuine ground incline.  The balance
controller subtracts the slope estimate from measured pitch so the PID
balances around the slope surface rather than absolute vertical.

- include/slope_estimator.h + src/slope_estimator.c: first-order IIR
  filter clamped to ±15°; JLINK_TLM_SLOPE (0x88) telemetry at 1 Hz
- include/jlink.h + src/jlink.c: add JLINK_TLM_SLOPE (0x88),
  jlink_tlm_slope_t (4 bytes), jlink_send_slope_tlm()
- include/balance.h + src/balance.c: integrate slope_estimator into
  balance_t; update, reset on tilt-fault and disarm
- test/test_slope_estimator.c: 35 unit tests, all passing

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
- peer_range_once(): DS-TWR initiator role toward a peer anchor
  (POLL → RESP → FINAL, one-sided range estimate Ra - Da/2)
- AT+PEER_RANGE=<id>: returns +PEER_RANGE:<my>,<peer>,<mm>,<rssi>
  or +PEER_RANGE:ERR,<peer>,TIMEOUT

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 6c00d6a321 into main 2026-03-14 15:54:57 -04:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#608
No description provided.
Delete Branch "sl-uwb/issue-602-anchor-calibration"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?