feat: Slope tilt compensation (Issue #600) #609
@ -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