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>
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 */
|