sl-controls
5f03e4cbef
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>
83 lines
2.6 KiB
C
83 lines
2.6 KiB
C
/*
|
||
* slope_estimator.c — slow-adapting terrain slope estimator (Issue #600).
|
||
*
|
||
* First-order IIR low-pass filter on fused IMU pitch with tau = SLOPE_TAU_S (5 s).
|
||
* The long time constant means fast balance corrections and transients are
|
||
* ignored; only sustained pitch bias (i.e., genuine slope) accumulates.
|
||
*
|
||
* Estimate is clamped to ±SLOPE_MAX_DEG (15°) per the safety requirement.
|
||
*/
|
||
|
||
#include "slope_estimator.h"
|
||
#include "jlink.h"
|
||
|
||
/* ---- slope_estimator_init() ---- */
|
||
void slope_estimator_init(slope_estimator_t *se)
|
||
{
|
||
se->estimate_deg = 0.0f;
|
||
se->enabled = true;
|
||
/* Initialize so first send_tlm call fires immediately */
|
||
se->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (SLOPE_TLM_HZ > 0u ? SLOPE_TLM_HZ : 1u)));
|
||
}
|
||
|
||
/* ---- slope_estimator_reset() ---- */
|
||
void slope_estimator_reset(slope_estimator_t *se)
|
||
{
|
||
se->estimate_deg = 0.0f;
|
||
}
|
||
|
||
/* ---- slope_estimator_update() ---- */
|
||
void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt)
|
||
{
|
||
if (!se->enabled || dt <= 0.0f) return;
|
||
|
||
/*
|
||
* First-order IIR: alpha = dt / (tau + dt)
|
||
* At 1 kHz (dt=0.001): alpha ≈ 0.0002 → time to ~63% ≈ 5 s
|
||
* At 100 Hz (dt=0.01): alpha ≈ 0.002 → time to ~63% ≈ 5 s (same tau)
|
||
*/
|
||
float alpha = dt / (SLOPE_TAU_S + dt);
|
||
float raw = se->estimate_deg * (1.0f - alpha) + pitch_deg * alpha;
|
||
|
||
/* Clamp to ±SLOPE_MAX_DEG */
|
||
if (raw > SLOPE_MAX_DEG) raw = SLOPE_MAX_DEG;
|
||
if (raw < -SLOPE_MAX_DEG) raw = -SLOPE_MAX_DEG;
|
||
|
||
se->estimate_deg = raw;
|
||
}
|
||
|
||
/* ---- slope_estimator_get_deg() ---- */
|
||
float slope_estimator_get_deg(const slope_estimator_t *se)
|
||
{
|
||
if (!se->enabled) return 0.0f;
|
||
return se->estimate_deg;
|
||
}
|
||
|
||
/* ---- slope_estimator_set_enabled() ---- */
|
||
void slope_estimator_set_enabled(slope_estimator_t *se, bool en)
|
||
{
|
||
se->enabled = en;
|
||
}
|
||
|
||
/* ---- slope_estimator_send_tlm() ---- */
|
||
void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms)
|
||
{
|
||
if (SLOPE_TLM_HZ == 0u) return;
|
||
|
||
uint32_t interval_ms = 1000u / SLOPE_TLM_HZ;
|
||
if ((now_ms - se->last_tlm_ms) < interval_ms) return;
|
||
/* Cast away const for timestamp update -- only mutable field */
|
||
((slope_estimator_t *)se)->last_tlm_ms = now_ms;
|
||
|
||
jlink_tlm_slope_t tlm;
|
||
/* Scale to ×100 for 0.01° resolution, clamped to int16 range */
|
||
float scaled = se->estimate_deg * 100.0f;
|
||
if (scaled > 32767.0f) scaled = 32767.0f;
|
||
if (scaled < -32768.0f) scaled = -32768.0f;
|
||
tlm.slope_x100 = (int16_t)scaled;
|
||
tlm.active = se->enabled ? 1u : 0u;
|
||
tlm._pad = 0u;
|
||
|
||
jlink_send_slope_tlm(&tlm);
|
||
}
|