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

3 Commits

Author SHA1 Message Date
sl-uwb
82cc223fb8 feat: Add AT+PEER_RANGE= command for inter-anchor calibration (Issue #602) 
- 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>
 2026-03-14 15:06:29 -04:00
sl-controls
5f03e4cbef feat: Tilt compensation for slopes (Issue #600) 
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>
 2026-03-14 15:04:58 -04:00
sl-uwb
587ca8a98e feat: UWB anchor auto-calibration via inter-anchor ranging + MDS (Issue #602) 
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>
 2026-03-14 15:03:53 -04:00