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>
This commit is contained in:
parent
061189670a
commit
be4966b01d
@ -3,6 +3,7 @@
|
||||
|
||||
#include <stdint.h>
|
||||
#include "mpu6000.h"
|
||||
#include "slope_estimator.h"
|
||||
|
||||
/*
|
||||
* SaltyLab Balance Controller
|
||||
@ -36,6 +37,9 @@ typedef struct {
|
||||
/* Safety */
|
||||
float max_tilt; /* Cutoff angle (degrees) */
|
||||
int16_t max_speed; /* Speed limit */
|
||||
|
||||
/* Slope compensation (Issue #600) */
|
||||
slope_estimator_t slope;
|
||||
} balance_t;
|
||||
|
||||
void balance_init(balance_t *b);
|
||||
|
||||
@ -87,6 +87,7 @@
|
||||
#define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */
|
||||
#define JLINK_TLM_MOTOR_CURRENT 0x86u /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */
|
||||
#define JLINK_TLM_FAULT_LOG 0x87u /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
|
||||
#define JLINK_TLM_SLOPE 0x88u /* jlink_tlm_slope_t (4 bytes, Issue #600) */
|
||||
|
||||
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
||||
typedef struct __attribute__((packed)) {
|
||||
@ -164,6 +165,14 @@ typedef struct __attribute__((packed)) {
|
||||
uint8_t _pad; /* reserved */
|
||||
} jlink_tlm_motor_current_t; /* 8 bytes */
|
||||
|
||||
/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */
|
||||
/* Published at SLOPE_TLM_HZ (1 Hz); reports slow-adapting terrain slope estimate. */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int16_t slope_x100; /* slope estimate degrees x100 (0.01° resolution) */
|
||||
uint8_t active; /* 1 = slope compensation enabled */
|
||||
uint8_t _pad; /* reserved */
|
||||
} jlink_tlm_slope_t; /* 4 bytes */
|
||||
|
||||
/* ---- Telemetry FAULT_LOG payload (20 bytes, packed) Issue #565 ---- */
|
||||
/* Sent on boot (if last fault != NONE) and in response to FAULT_LOG_GET. */
|
||||
typedef struct __attribute__((packed)) {
|
||||
@ -272,6 +281,13 @@ JLinkSchedSetBuf *jlink_get_sched_set(void);
|
||||
*/
|
||||
void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm);
|
||||
|
||||
/*
|
||||
* jlink_send_slope_tlm(tlm) - transmit JLINK_TLM_SLOPE (0x88) frame
|
||||
* (10 bytes total) to Jetson. Issue #600.
|
||||
* Rate-limiting is handled by slope_estimator_send_tlm(); call from there only.
|
||||
*/
|
||||
void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
|
||||
|
||||
/*
|
||||
* jlink_send_fault_log(fl) - transmit JLINK_TLM_FAULT_LOG (0x87) frame
|
||||
* (26 bytes) on boot (if fault log non-empty) and in response to
|
||||
|
||||
101
include/slope_estimator.h
Normal file
101
include/slope_estimator.h
Normal file
@ -0,0 +1,101 @@
|
||||
#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 */
|
||||
@ -1,4 +1,5 @@
|
||||
#include "balance.h"
|
||||
#include "slope_estimator.h"
|
||||
#include "config.h"
|
||||
#include <math.h>
|
||||
|
||||
@ -19,6 +20,9 @@ void balance_init(balance_t *b) {
|
||||
/* Safety defaults from config */
|
||||
b->max_tilt = MAX_TILT_DEG;
|
||||
b->max_speed = MAX_SPEED_LIMIT;
|
||||
|
||||
/* Slope estimator */
|
||||
slope_estimator_init(&b->slope);
|
||||
}
|
||||
|
||||
void balance_update(balance_t *b, const IMUData *imu, float dt) {
|
||||
@ -28,12 +32,16 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) {
|
||||
b->pitch_deg = imu->pitch;
|
||||
b->pitch_rate = imu->pitch_rate;
|
||||
|
||||
/* Advance slope estimator (no-op when not armed) */
|
||||
slope_estimator_update(&b->slope, b->pitch_deg, dt);
|
||||
|
||||
/* Safety: tilt cutoff */
|
||||
if (b->state == BALANCE_ARMED) {
|
||||
if (fabsf(b->pitch_deg) > b->max_tilt) {
|
||||
b->state = BALANCE_TILT_FAULT;
|
||||
b->motor_cmd = 0;
|
||||
b->integral = 0.0f;
|
||||
slope_estimator_reset(&b->slope);
|
||||
return;
|
||||
}
|
||||
}
|
||||
@ -45,8 +53,9 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* PID */
|
||||
float error = b->setpoint - b->pitch_deg;
|
||||
/* PID — subtract slope estimate so controller balances on incline */
|
||||
float tilt_corrected = b->pitch_deg - slope_estimator_get_deg(&b->slope);
|
||||
float error = b->setpoint - tilt_corrected;
|
||||
|
||||
/* Proportional */
|
||||
float p_term = b->kp * error;
|
||||
@ -85,4 +94,5 @@ void balance_disarm(balance_t *b) {
|
||||
b->state = BALANCE_DISARMED;
|
||||
b->motor_cmd = 0;
|
||||
b->integral = 0.0f;
|
||||
slope_estimator_reset(&b->slope);
|
||||
}
|
||||
|
||||
24
src/jlink.c
24
src/jlink.c
@ -541,6 +541,30 @@ void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm)
|
||||
jlink_tx_locked(frame, (uint16_t)(3u + plen + 3u));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_slope_tlm() -- Issue #600 ---- */
|
||||
void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x88][4 bytes SLOPE][CRC_hi][CRC_lo][ETX]
|
||||
* Total: 1+1+1+4+2+1 = 10 bytes
|
||||
*/
|
||||
static uint8_t frame[10];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_slope_t); /* 4 */
|
||||
const uint8_t len = 1u + plen; /* CMD byte + payload */
|
||||
|
||||
frame[0] = JLINK_STX;
|
||||
frame[1] = len;
|
||||
frame[2] = JLINK_TLM_SLOPE;
|
||||
memcpy(&frame[3], tlm, plen);
|
||||
|
||||
uint16_t crc = crc16_xmodem(&frame[2], len);
|
||||
frame[3 + plen] = (uint8_t)(crc >> 8);
|
||||
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
|
||||
frame[3 + plen + 2] = JLINK_ETX;
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_fault_log() -- Issue #565 ---- */
|
||||
void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl)
|
||||
{
|
||||
|
||||
82
src/slope_estimator.c
Normal file
82
src/slope_estimator.c
Normal file
@ -0,0 +1,82 @@
|
||||
/*
|
||||
* 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);
|
||||
}
|
||||
421
test/test_slope_estimator.c
Normal file
421
test/test_slope_estimator.c
Normal file
@ -0,0 +1,421 @@
|
||||
/*
|
||||
* test_slope_estimator.c — Unit tests for slope estimator (Issue #600).
|
||||
*
|
||||
* Build (host, no hardware):
|
||||
* gcc -I include -I test/stubs -DTEST_HOST -lm \
|
||||
* -o /tmp/test_slope src/slope_estimator.c test/test_slope_estimator.c
|
||||
*
|
||||
* All tests are self-contained; no HAL, no ADC, no UART required.
|
||||
* slope_estimator.c calls jlink_send_slope_tlm() which is stubbed below.
|
||||
*/
|
||||
|
||||
/* ---- Stubs ---- */
|
||||
|
||||
/* Prevent jlink.h from pulling in HAL / pid_flash types */
|
||||
#define JLINK_H
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* Minimal jlink_tlm_slope_t matching the real definition */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int16_t slope_x100;
|
||||
uint8_t active;
|
||||
uint8_t _pad;
|
||||
} jlink_tlm_slope_t;
|
||||
|
||||
/* Capture last transmitted tlm for inspection */
|
||||
static jlink_tlm_slope_t g_last_tlm;
|
||||
static int g_tlm_count = 0;
|
||||
|
||||
void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm)
|
||||
{
|
||||
g_last_tlm = *tlm;
|
||||
g_tlm_count++;
|
||||
}
|
||||
|
||||
/* ---- Include implementation directly ---- */
|
||||
#include "../src/slope_estimator.c"
|
||||
|
||||
/* ---- Test framework ---- */
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
static int g_pass = 0;
|
||||
static int g_fail = 0;
|
||||
|
||||
#define ASSERT(cond, msg) do { \
|
||||
if (cond) { g_pass++; } \
|
||||
else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \
|
||||
} while(0)
|
||||
|
||||
#define ASSERT_NEAR(a, b, tol, msg) \
|
||||
ASSERT(fabsf((a)-(b)) <= (tol), msg)
|
||||
|
||||
/* ---- Tests ---- */
|
||||
|
||||
static void test_init_zeroes_state(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
/* dirty the memory first */
|
||||
memset(&se, 0xAB, sizeof(se));
|
||||
slope_estimator_init(&se);
|
||||
ASSERT(se.estimate_deg == 0.0f, "init: estimate_deg == 0");
|
||||
ASSERT(se.enabled == true, "init: enabled == true");
|
||||
}
|
||||
|
||||
static void test_get_when_disabled_returns_zero(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
/* Force a non-zero estimate by running a few ticks, then disable */
|
||||
slope_estimator_update(&se, 10.0f, 0.1f);
|
||||
slope_estimator_set_enabled(&se, false);
|
||||
ASSERT(slope_estimator_get_deg(&se) == 0.0f,
|
||||
"get while disabled must return 0");
|
||||
}
|
||||
|
||||
static void test_update_when_disabled_no_change(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
slope_estimator_set_enabled(&se, false);
|
||||
slope_estimator_update(&se, 10.0f, 0.01f);
|
||||
ASSERT(se.estimate_deg == 0.0f,
|
||||
"update while disabled must not change estimate");
|
||||
}
|
||||
|
||||
static void test_update_zero_dt_no_change(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
slope_estimator_update(&se, 5.0f, 0.0f);
|
||||
ASSERT(se.estimate_deg == 0.0f,
|
||||
"update with dt=0 must not change estimate");
|
||||
}
|
||||
|
||||
static void test_update_negative_dt_no_change(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
slope_estimator_update(&se, 5.0f, -0.001f);
|
||||
ASSERT(se.estimate_deg == 0.0f,
|
||||
"update with dt<0 must not change estimate");
|
||||
}
|
||||
|
||||
static void test_single_step_converges_toward_input(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
/* One update with dt=0.01s, pitch=10 deg */
|
||||
slope_estimator_update(&se, 10.0f, 0.01f);
|
||||
/* alpha = 0.01 / (5.0 + 0.01) ≈ 0.002; new estimate ≈ 0 + 0.002*10 = 0.02 */
|
||||
ASSERT(se.estimate_deg > 0.0f,
|
||||
"estimate should move toward positive pitch");
|
||||
ASSERT(se.estimate_deg < 10.0f,
|
||||
"estimate should not jump to full pitch in one step");
|
||||
}
|
||||
|
||||
static void test_iir_reaches_63pct_in_one_tau(void)
|
||||
{
|
||||
/* Run estimator at 1 kHz for exactly SLOPE_TAU_S seconds */
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
const float pitch = 10.0f;
|
||||
const float dt = 0.001f;
|
||||
const int steps = (int)(SLOPE_TAU_S / dt); /* 5000 steps */
|
||||
for (int i = 0; i < steps; i++) {
|
||||
slope_estimator_update(&se, pitch, dt);
|
||||
}
|
||||
/* IIR should be at ≈63.2% of the step */
|
||||
float expected_lo = pitch * 0.60f;
|
||||
float expected_hi = pitch * 0.66f;
|
||||
ASSERT(se.estimate_deg >= expected_lo && se.estimate_deg <= expected_hi,
|
||||
"IIR should reach ~63% of step at t=tau");
|
||||
}
|
||||
|
||||
static void test_iir_reaches_63pct_at_100hz(void)
|
||||
{
|
||||
/* Same tau, but at 100 Hz */
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
const float pitch = 8.0f;
|
||||
const float dt = 0.01f;
|
||||
const int steps = (int)(SLOPE_TAU_S / dt); /* 500 steps */
|
||||
for (int i = 0; i < steps; i++) {
|
||||
slope_estimator_update(&se, pitch, dt);
|
||||
}
|
||||
float expected_lo = pitch * 0.60f;
|
||||
float expected_hi = pitch * 0.66f;
|
||||
ASSERT(se.estimate_deg >= expected_lo && se.estimate_deg <= expected_hi,
|
||||
"IIR 100 Hz: should reach ~63% at t=tau");
|
||||
}
|
||||
|
||||
static void test_positive_clamp(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
/* Drive with a huge pitch for a long time — estimate must clamp at SLOPE_MAX_DEG */
|
||||
for (int i = 0; i < 100000; i++) {
|
||||
slope_estimator_update(&se, 90.0f, 0.001f);
|
||||
}
|
||||
ASSERT_NEAR(se.estimate_deg, SLOPE_MAX_DEG, 0.001f,
|
||||
"estimate must clamp at +SLOPE_MAX_DEG");
|
||||
}
|
||||
|
||||
static void test_negative_clamp(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
for (int i = 0; i < 100000; i++) {
|
||||
slope_estimator_update(&se, -90.0f, 0.001f);
|
||||
}
|
||||
ASSERT_NEAR(se.estimate_deg, -SLOPE_MAX_DEG, 0.001f,
|
||||
"estimate must clamp at -SLOPE_MAX_DEG");
|
||||
}
|
||||
|
||||
static void test_reset_zeroes_estimate(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
/* Build up some estimate */
|
||||
for (int i = 0; i < 1000; i++) {
|
||||
slope_estimator_update(&se, 12.0f, 0.001f);
|
||||
}
|
||||
ASSERT(se.estimate_deg > 0.1f, "pre-reset: estimate should be non-zero");
|
||||
slope_estimator_reset(&se);
|
||||
ASSERT(se.estimate_deg == 0.0f, "post-reset: estimate should be zero");
|
||||
/* enabled state must be preserved */
|
||||
ASSERT(se.enabled == true, "reset must not change enabled flag");
|
||||
}
|
||||
|
||||
static void test_reset_preserves_disabled_state(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
slope_estimator_set_enabled(&se, false);
|
||||
slope_estimator_reset(&se);
|
||||
ASSERT(se.enabled == false, "reset must not re-enable disabled estimator");
|
||||
}
|
||||
|
||||
static void test_balance_setpoint_compensation(void)
|
||||
{
|
||||
/*
|
||||
* Simulate: robot on 5-deg slope, pitch = 5 deg, slope estimate converges.
|
||||
* After convergence, tilt_corrected = pitch - slope ≈ 0.
|
||||
* Verify that the effective PID error (setpoint=0, tilt_corrected≈0) is near 0.
|
||||
*/
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
const float slope_deg = 5.0f;
|
||||
const float dt = 0.001f;
|
||||
const int steps = (int)(3.0f * SLOPE_TAU_S / dt); /* 3 tau = ~95% converged */
|
||||
|
||||
for (int i = 0; i < steps; i++) {
|
||||
/* Robot holds steady on slope — pitch stays constant */
|
||||
slope_estimator_update(&se, slope_deg, dt);
|
||||
}
|
||||
|
||||
float est = slope_estimator_get_deg(&se);
|
||||
float tilt_corrected = slope_deg - est;
|
||||
|
||||
/* After 3 tau, estimate should be within 5% of slope */
|
||||
ASSERT(est >= slope_deg * 0.90f, "3-tau estimate >= 90% of slope");
|
||||
/* tilt_corrected should be close to zero — minimal PID error */
|
||||
ASSERT(fabsf(tilt_corrected) < slope_deg * 0.10f,
|
||||
"corrected tilt error < 10% of slope after 3 tau");
|
||||
}
|
||||
|
||||
static void test_fast_tilt_not_tracked(void)
|
||||
{
|
||||
/*
|
||||
* Simulate: robot starts balanced (pitch≈0), then tips to 20 deg suddenly
|
||||
* for 100 ms (balance intervention), then returns to 0.
|
||||
* The slope estimate should not move significantly.
|
||||
*/
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
const float dt = 0.001f;
|
||||
|
||||
/* 2 s of stable balancing at pitch ≈ 0 */
|
||||
for (int i = 0; i < 2000; i++) {
|
||||
slope_estimator_update(&se, 0.0f, dt);
|
||||
}
|
||||
float before = se.estimate_deg;
|
||||
|
||||
/* 100 ms spike to 20 deg (fast tilt) */
|
||||
for (int i = 0; i < 100; i++) {
|
||||
slope_estimator_update(&se, 20.0f, dt);
|
||||
}
|
||||
/* Return to 0 for another 100 ms */
|
||||
for (int i = 0; i < 100; i++) {
|
||||
slope_estimator_update(&se, 0.0f, dt);
|
||||
}
|
||||
float after = se.estimate_deg;
|
||||
|
||||
/* 200ms / 5000ms = 4% of tau — estimate should move < 1 deg */
|
||||
ASSERT(fabsf(after - before) < 1.0f,
|
||||
"fast tilt transient should not significantly move slope estimate");
|
||||
}
|
||||
|
||||
static void test_slope_change_tracks_slowly(void)
|
||||
{
|
||||
/* Robot transitions from flat (0°) to 10° slope at t=0.
|
||||
* After 1 tau, estimate should be ~63% of 10 = ~6.3 deg. */
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
const float dt = 0.001f;
|
||||
const int steps = (int)(SLOPE_TAU_S / dt);
|
||||
|
||||
for (int i = 0; i < steps; i++) {
|
||||
slope_estimator_update(&se, 10.0f, dt);
|
||||
}
|
||||
ASSERT(se.estimate_deg >= 6.0f && se.estimate_deg <= 6.5f,
|
||||
"slope change: 1-tau estimate should be 6.0-6.5 deg");
|
||||
}
|
||||
|
||||
static void test_symmetry_negative_slope(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
const float dt = 0.001f;
|
||||
const int steps = (int)(SLOPE_TAU_S / dt);
|
||||
|
||||
for (int i = 0; i < steps; i++) {
|
||||
slope_estimator_update(&se, -8.0f, dt);
|
||||
}
|
||||
/* Should be ~63% of -8 */
|
||||
ASSERT(se.estimate_deg <= -4.0f && se.estimate_deg >= -5.5f,
|
||||
"negative slope: estimate should converge symmetrically");
|
||||
}
|
||||
|
||||
static void test_enable_disable_toggle(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
|
||||
/* Build estimate */
|
||||
for (int i = 0; i < 2000; i++) {
|
||||
slope_estimator_update(&se, 10.0f, 0.001f);
|
||||
}
|
||||
float val = se.estimate_deg;
|
||||
ASSERT(val > 0.1f, "estimate built before disable");
|
||||
|
||||
/* Disable: estimate frozen */
|
||||
slope_estimator_set_enabled(&se, false);
|
||||
slope_estimator_update(&se, 10.0f, 0.001f);
|
||||
ASSERT(se.estimate_deg == val, "estimate frozen while disabled");
|
||||
|
||||
/* get returns 0 when disabled */
|
||||
ASSERT(slope_estimator_get_deg(&se) == 0.0f, "get returns 0 while disabled");
|
||||
|
||||
/* Re-enable: estimate resumes */
|
||||
slope_estimator_set_enabled(&se, true);
|
||||
slope_estimator_update(&se, 10.0f, 0.001f);
|
||||
ASSERT(se.estimate_deg > val, "estimate resumes after re-enable");
|
||||
}
|
||||
|
||||
static void test_tlm_rate_limiting(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
|
||||
g_tlm_count = 0;
|
||||
|
||||
/* Call at 1000 Hz for 3 seconds → should send 3 TLMs (at 1 Hz rate limit) */
|
||||
for (uint32_t ms = 0; ms < 3000; ms++) {
|
||||
slope_estimator_send_tlm(&se, ms);
|
||||
}
|
||||
/* First call at ms=0 sends, then 1000ms, 2000ms → 3 total */
|
||||
ASSERT(g_tlm_count == 3, "TLM rate-limited to SLOPE_TLM_HZ (1 Hz)");
|
||||
}
|
||||
|
||||
static void test_tlm_payload_encoding(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
|
||||
/* Force a known estimate */
|
||||
se.estimate_deg = 7.5f;
|
||||
se.enabled = true;
|
||||
|
||||
g_tlm_count = 0;
|
||||
g_last_tlm.slope_x100 = 0;
|
||||
|
||||
/* Trigger a TLM by advancing time enough */
|
||||
slope_estimator_send_tlm(&se, 0u);
|
||||
|
||||
ASSERT(g_tlm_count == 1, "TLM sent");
|
||||
ASSERT(g_last_tlm.slope_x100 == 750, "7.5 deg -> slope_x100 = 750");
|
||||
ASSERT(g_last_tlm.active == 1u, "active flag set when enabled");
|
||||
}
|
||||
|
||||
static void test_tlm_disabled_active_flag(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
slope_estimator_set_enabled(&se, false);
|
||||
se.estimate_deg = 3.0f;
|
||||
|
||||
g_tlm_count = 0;
|
||||
slope_estimator_send_tlm(&se, 0u);
|
||||
|
||||
ASSERT(g_tlm_count == 1, "TLM sent when disabled");
|
||||
ASSERT(g_last_tlm.active == 0u, "active flag clear when disabled");
|
||||
/* slope_x100 reflects stored estimate but get() returns 0 */
|
||||
ASSERT(g_last_tlm.slope_x100 == 300, "slope_x100 encodes stored estimate");
|
||||
}
|
||||
|
||||
static void test_tlm_clamped_to_int16(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
/* Manually set beyond int16 range for test */
|
||||
se.estimate_deg = 400.0f; /* 400 * 100 = 40000, within int16 */
|
||||
se.enabled = true;
|
||||
g_tlm_count = 0;
|
||||
slope_estimator_send_tlm(&se, 0u);
|
||||
ASSERT(g_last_tlm.slope_x100 == 32767, "large estimate clamped to INT16_MAX");
|
||||
}
|
||||
|
||||
static void test_multiple_resets_idempotent(void)
|
||||
{
|
||||
slope_estimator_t se;
|
||||
slope_estimator_init(&se);
|
||||
slope_estimator_reset(&se);
|
||||
slope_estimator_reset(&se);
|
||||
ASSERT(se.estimate_deg == 0.0f, "multiple resets: still zero");
|
||||
ASSERT(se.enabled == true, "multiple resets: still enabled");
|
||||
}
|
||||
|
||||
/* ---- main ---- */
|
||||
int main(void)
|
||||
{
|
||||
printf("=== test_slope_estimator ===\n");
|
||||
|
||||
test_init_zeroes_state();
|
||||
test_get_when_disabled_returns_zero();
|
||||
test_update_when_disabled_no_change();
|
||||
test_update_zero_dt_no_change();
|
||||
test_update_negative_dt_no_change();
|
||||
test_single_step_converges_toward_input();
|
||||
test_iir_reaches_63pct_in_one_tau();
|
||||
test_iir_reaches_63pct_at_100hz();
|
||||
test_positive_clamp();
|
||||
test_negative_clamp();
|
||||
test_reset_zeroes_estimate();
|
||||
test_reset_preserves_disabled_state();
|
||||
test_balance_setpoint_compensation();
|
||||
test_fast_tilt_not_tracked();
|
||||
test_slope_change_tracks_slowly();
|
||||
test_symmetry_negative_slope();
|
||||
test_enable_disable_toggle();
|
||||
test_tlm_rate_limiting();
|
||||
test_tlm_payload_encoding();
|
||||
test_tlm_disabled_active_flag();
|
||||
test_tlm_clamped_to_int16();
|
||||
test_multiple_resets_idempotent();
|
||||
|
||||
printf("\nResults: %d passed, %d failed\n", g_pass, g_fail);
|
||||
return g_fail ? 1 : 0;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user