sl-controls
be4966b01d
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>
102 lines
3.7 KiB
C
102 lines
3.7 KiB
C
#ifndef SLOPE_ESTIMATOR_H
|
|
#define SLOPE_ESTIMATOR_H
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
|
|
/*
|
|
* slope_estimator — slow-adapting terrain slope estimator for Issue #600.
|
|
*
|
|
* On a slope the robot must lean slightly into the hill to stay balanced.
|
|
* The IMU pitch reading therefore includes both the robot's balance offset
|
|
* and the ground incline. This module decouples the two by tracking the
|
|
* slowly-changing DC component of the pitch signal with a first-order IIR
|
|
* low-pass filter (time constant SLOPE_TAU_S = 5 s).
|
|
*
|
|
* HOW IT WORKS:
|
|
* Every call to slope_estimator_update(pitch_deg, dt):
|
|
*
|
|
* alpha = dt / (SLOPE_TAU_S + dt) // ≈ 0.0002 at 1 kHz
|
|
* raw = slope * (1 - alpha) + pitch * alpha
|
|
* slope = clamp(raw, -SLOPE_MAX_DEG, +SLOPE_MAX_DEG)
|
|
*
|
|
* The IIR converges to the steady-state pitch in ~5 s. Fast tilt
|
|
* transients (balance corrections, steps, bumps) are attenuated by
|
|
* the long time constant and do not corrupt the estimate.
|
|
*
|
|
* INTEGRATION IN BALANCE PID:
|
|
* Subtract slope_estimate from the measured pitch before computing
|
|
* the PID error so the controller balances around the slope surface
|
|
* rather than absolute vertical:
|
|
*
|
|
* tilt_corrected = pitch_deg - slope_estimate_deg
|
|
* error = setpoint - tilt_corrected
|
|
*
|
|
* This is equivalent to continuously adjusting the balance setpoint
|
|
* to track the incline.
|
|
*
|
|
* SAFETY:
|
|
* - Estimate is clamped to ±SLOPE_MAX_DEG (15°) to prevent drift from
|
|
* extreme falls being mistaken for genuine slopes.
|
|
* - slope_estimator_reset() zeroes the state; call on disarm or after
|
|
* a tilt fault so re-arming starts fresh.
|
|
*
|
|
* TELEMETRY:
|
|
* JLINK_TLM_SLOPE (0x88) published at SLOPE_TLM_HZ (1 Hz):
|
|
* jlink_tlm_slope_t { int16 slope_x100, uint8 active, uint8 _pad }
|
|
*/
|
|
|
|
/* ---- Configuration ---- */
|
|
#define SLOPE_TAU_S 5.0f /* IIR time constant (seconds) */
|
|
#define SLOPE_MAX_DEG 15.0f /* Maximum estimate magnitude (degrees) */
|
|
#define SLOPE_TLM_HZ 1u /* JLINK_TLM_SLOPE publish rate (Hz) */
|
|
|
|
/* ---- State ---- */
|
|
typedef struct {
|
|
float estimate_deg; /* current slope estimate (degrees, + = nose-up) */
|
|
bool enabled; /* compensation on/off; off = estimate frozen */
|
|
uint32_t last_tlm_ms; /* timestamp of last TLM transmission */
|
|
} slope_estimator_t;
|
|
|
|
/* ---- API ---- */
|
|
|
|
/*
|
|
* slope_estimator_init(se) — zero state, enable estimation.
|
|
* Call once during system init.
|
|
*/
|
|
void slope_estimator_init(slope_estimator_t *se);
|
|
|
|
/*
|
|
* slope_estimator_reset(se) — zero estimate without changing enabled flag.
|
|
* Call on disarm or after BALANCE_TILT_FAULT to avoid stale state on rearm.
|
|
*/
|
|
void slope_estimator_reset(slope_estimator_t *se);
|
|
|
|
/*
|
|
* slope_estimator_update(se, pitch_deg, dt) — advance the IIR filter.
|
|
* pitch_deg : current fused pitch angle from IMU (degrees)
|
|
* dt : loop interval (seconds)
|
|
* No-op if se->enabled == false or dt <= 0.
|
|
*/
|
|
void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt);
|
|
|
|
/*
|
|
* slope_estimator_get_deg(se) — return current estimate (degrees).
|
|
* Returns 0 if disabled.
|
|
*/
|
|
float slope_estimator_get_deg(const slope_estimator_t *se);
|
|
|
|
/*
|
|
* slope_estimator_set_enabled(se, en) — enable or disable compensation.
|
|
* Disabling freezes the estimate at its current value.
|
|
*/
|
|
void slope_estimator_set_enabled(slope_estimator_t *se, bool en);
|
|
|
|
/*
|
|
* slope_estimator_send_tlm(se, now_ms) — transmit JLINK_TLM_SLOPE (0x88)
|
|
* frame to Jetson. Rate-limited to SLOPE_TLM_HZ; safe to call every tick.
|
|
*/
|
|
void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms);
|
|
|
|
#endif /* SLOPE_ESTIMATOR_H */
|