sl-firmware
fa75c442a7
Archive STM32 firmware to legacy/stm32/: - src/, include/, lib/USB_CDC/, platformio.ini, test stubs, flash_firmware.py - test/test_battery_adc.c, test_hw_button.c, test_pid_schedule.c, test_vesc_can.c, test_can_watchdog.c - USB_CDC_BUG.md Rename: stm32_protocol → esp32_protocol, mamba_protocol → balance_protocol, stm32_cmd_node → esp32_cmd_node, stm32_cmd_params → esp32_cmd_params, stm32_cmd.launch.py → esp32_cmd.launch.py, test_stm32_protocol → test_esp32_protocol, test_stm32_cmd_node → test_esp32_cmd_node Content cleanup across all files: - Mamba F722S → ESP32-S3 BALANCE - BlackPill → ESP32-S3 IO - STM32F722/F7xx → ESP32-S3 - stm32Mode/Version/Port → esp32Mode/Version/Port - STM32 State/Mode labels → ESP32 State/Mode - Jetson Nano → Jetson Orin Nano Super - /dev/stm32 → /dev/esp32 - stm32_bridge → esp32_bridge - STM32 HAL → ESP-IDF docs/SALTYLAB.md: - Update "Drone FC Details" to describe ESP32-S3 BALANCE board (Waveshare ESP32-S3 Touch LCD 1.28) - Replace verbose "Self-Balancing Control" STM32 section with brief note pointing to SAUL-TEE-SYSTEM-REFERENCE.md TEAM.md: Update Embedded Firmware Engineer role to ESP32-S3 / ESP-IDF No new functionality — cleanup only. 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);
|
||
}
|