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

feat: Slope tilt compensation (Issue #600) #609

Merged
sl-jetson merged 1 commits from sl-controls/issue-600-slope-compensation into main 2026-03-14 15:55:02 -04:00
Conversation 0 Commits 1 Files Changed 7 +660 -2
sl-perception commented 2026-03-14 15:07:24 -04:00
Collaborator
Copy Link

Implements Issue #600: slow-adapting terrain slope estimator so the balance controller can maintain upright posture on inclined surfaces.

Changes

New: include/slope_estimator.h + src/slope_estimator.c

  • First-order IIR low-pass filter (τ=5s) on fused IMU pitch
  • Clamped to ±15° to prevent drift from falls being mistaken for slopes
  • slope_estimator_init/reset/update/get_deg/set_enabled/send_tlm API
  • JLINK_TLM_SLOPE (0x88) telemetry at 1 Hz: slope_x100 (int16) + active flag

Modified: include/jlink.h + src/jlink.c

  • Added JLINK_TLM_SLOPE 0x88u telemetry ID
  • Added jlink_tlm_slope_t (4 bytes packed)
  • Added jlink_send_slope_tlm() implementation (10-byte frame)

Modified: include/balance.h + src/balance.c

  • Added slope_estimator_t slope field to balance_t
  • balance_init(): calls slope_estimator_init()
  • balance_update(): advances IIR each tick; subtracts estimate from pitch before PID error: tilt_corrected = pitch_deg - slope_estimate_deg
  • Tilt-fault and disarm paths call slope_estimator_reset() to avoid stale state on re-arm

New: test/test_slope_estimator.c

  • 35 unit tests (host build with gcc): IIR convergence at 1 kHz and 100 Hz, ±clamp, disabled behaviour, balance setpoint compensation, TLM rate-limiting, payload encoding, INT16 edge cases — all passing

Closes #600

Implements Issue #600: slow-adapting terrain slope estimator so the balance controller can maintain upright posture on inclined surfaces. ## Changes ### New: `include/slope_estimator.h` + `src/slope_estimator.c` - First-order IIR low-pass filter (τ=5s) on fused IMU pitch - Clamped to ±15° to prevent drift from falls being mistaken for slopes - `slope_estimator_init/reset/update/get_deg/set_enabled/send_tlm` API - JLINK_TLM_SLOPE (0x88) telemetry at 1 Hz: `slope_x100` (int16) + `active` flag ### Modified: `include/jlink.h` + `src/jlink.c` - Added `JLINK_TLM_SLOPE 0x88u` telemetry ID - Added `jlink_tlm_slope_t` (4 bytes packed) - Added `jlink_send_slope_tlm()` implementation (10-byte frame) ### Modified: `include/balance.h` + `src/balance.c` - Added `slope_estimator_t slope` field to `balance_t` - `balance_init()`: calls `slope_estimator_init()` - `balance_update()`: advances IIR each tick; subtracts estimate from pitch before PID error: `tilt_corrected = pitch_deg - slope_estimate_deg` - Tilt-fault and disarm paths call `slope_estimator_reset()` to avoid stale state on re-arm ### New: `test/test_slope_estimator.c` - 35 unit tests (host build with gcc): IIR convergence at 1 kHz and 100 Hz, ±clamp, disabled behaviour, balance setpoint compensation, TLM rate-limiting, payload encoding, INT16 edge cases — all passing Closes #600
sl-perception added 1 commit 2026-03-14 15:07:25 -04:00 
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>
sl-jetson merged commit 8b1d6483cf into main 2026-03-14 15:55:02 -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#609
No description provided.
Delete Branch "sl-controls/issue-600-slope-compensation"

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?