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>
123 lines
5.0 KiB
C
123 lines
5.0 KiB
C
/*
|
||
* pid_schedule.h — Speed-dependent PID gain scheduling (Issue #550)
|
||
*
|
||
* Maps robot velocity to PID gain triplets (Kp, Ki, Kd) using a lookup
|
||
* table with linear interpolation between adjacent entries. The table
|
||
* supports 1–PID_SCHED_MAX_BANDS entries, each associating a velocity
|
||
* breakpoint (m/s) with gains that apply AT that velocity.
|
||
*
|
||
* HOW IT WORKS:
|
||
* 1. Each entry in the table defines: {speed_mps, kp, ki, kd}.
|
||
* The table is sorted by speed_mps ascending (pid_schedule_set_table
|
||
* sorts automatically).
|
||
*
|
||
* 2. pid_schedule_get_gains(speed_mps, ...) finds the two adjacent entries
|
||
* that bracket the query speed and linearly interpolates:
|
||
* t = (speed - bands[i-1].speed_mps) /
|
||
* (bands[i].speed_mps - bands[i-1].speed_mps)
|
||
* kp = bands[i-1].kp + t * (bands[i].kp - bands[i-1].kp)
|
||
* Speeds below the first entry or above the last entry clamp to the
|
||
* nearest endpoint (no extrapolation).
|
||
* The query speed is ABS(motor_speed) — scheduling is symmetric.
|
||
*
|
||
* 3. Default 3-entry table (loaded when flash has no valid schedule):
|
||
* Band 0: speed=0.00 m/s kp=40.0 ki=1.5 kd=1.2 (stopped — tight)
|
||
* Band 1: speed=0.30 m/s kp=35.0 ki=1.0 kd=1.0 (slow — balanced)
|
||
* Band 2: speed=0.80 m/s kp=28.0 ki=0.5 kd=0.8 (fast — relaxed)
|
||
*
|
||
* 4. pid_schedule_apply(balance, speed_mps) interpolates and writes the
|
||
* result directly into balance->kp/ki/kd. Call from the main loop at
|
||
* the same rate as the balance PID update (1 kHz) or slower (100 Hz
|
||
* for scheduling, 1 kHz for PID execution — gains change slowly enough).
|
||
*
|
||
* 5. Flash persistence: pid_schedule_flash_save() calls pid_flash_save_all()
|
||
* which erases sector 7 once and writes both the single-PID record at
|
||
* PID_FLASH_STORE_ADDR and the schedule at PID_SCHED_FLASH_ADDR.
|
||
*
|
||
* 6. JLINK interface (Issue #550):
|
||
* 0x0C SCHED_GET — no payload; triggers TLM_SCHED response
|
||
* 0x0D SCHED_SET — upload new table (num_bands + N×16-byte entries)
|
||
* 0x0E SCHED_SAVE — save current table + single PID to flash
|
||
* 0x85 TLM_SCHED — table dump response to SCHED_GET
|
||
*/
|
||
|
||
#ifndef PID_SCHEDULE_H
|
||
#define PID_SCHEDULE_H
|
||
|
||
#include <stdint.h>
|
||
#include <stdbool.h>
|
||
#include "pid_flash.h" /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */
|
||
#include "balance.h" /* balance_t */
|
||
|
||
/* ---- Default gain table ---- */
|
||
/* Motor ESC range is ±1000 counts; 1000 counts ≈ full drive.
|
||
* Speed scale: MOTOR_CMD_MAX=1000 → ~0.8 m/s max tangential velocity.
|
||
* Adjust PID_SCHED_SPEED_SCALE if odometry calibration changes this. */
|
||
#define PID_SCHED_SPEED_SCALE 0.0008f /* motor_cmd counts → m/s: 1000 × 0.0008 = 0.8 m/s */
|
||
|
||
/* ---- API ---- */
|
||
|
||
/*
|
||
* pid_schedule_init() — load table from flash (via pid_flash_load_schedule).
|
||
* Falls back to the built-in 3-band default if flash is empty or invalid.
|
||
* Call once after flash init during system startup.
|
||
*/
|
||
void pid_schedule_init(void);
|
||
|
||
/*
|
||
* pid_schedule_get_gains(speed_mps, *kp, *ki, *kd) — interpolate gains.
|
||
* |speed_mps| is used (scheduling is symmetric for forward/reverse).
|
||
* Clamps to table endpoints; does not extrapolate outside the table range.
|
||
*/
|
||
void pid_schedule_get_gains(float speed_mps, float *kp, float *ki, float *kd);
|
||
|
||
/*
|
||
* pid_schedule_apply(b, speed_mps) — compute interpolated gains and write
|
||
* them into b->kp, b->ki, b->kd. b->integral is reset to 0 when the
|
||
* active band changes to avoid integrator windup on transitions.
|
||
*/
|
||
void pid_schedule_apply(balance_t *b, float speed_mps);
|
||
|
||
/*
|
||
* pid_schedule_set_table(entries, n) — replace the active gain table.
|
||
* Entries are copied and sorted by speed_mps ascending.
|
||
* n is clamped to [1, PID_SCHED_MAX_BANDS].
|
||
* Does NOT automatically save to flash — call pid_schedule_flash_save().
|
||
*/
|
||
void pid_schedule_set_table(const pid_sched_entry_t *entries, uint8_t n);
|
||
|
||
/*
|
||
* pid_schedule_get_table(out_entries, out_n) — copy current table out.
|
||
* out_entries must have room for PID_SCHED_MAX_BANDS entries.
|
||
*/
|
||
void pid_schedule_get_table(pid_sched_entry_t *out_entries, uint8_t *out_n);
|
||
|
||
/*
|
||
* pid_schedule_get_num_bands() — return current number of table entries.
|
||
*/
|
||
uint8_t pid_schedule_get_num_bands(void);
|
||
|
||
/*
|
||
* pid_schedule_flash_save(kp_single, ki_single, kd_single) — save the
|
||
* current schedule table PLUS the caller-supplied single-PID values to
|
||
* flash in one atomic sector erase (pid_flash_save_all).
|
||
* Must NOT be called while armed (sector erase takes ~1s).
|
||
* Returns true on success.
|
||
*/
|
||
bool pid_schedule_flash_save(float kp_single, float ki_single, float kd_single);
|
||
|
||
/*
|
||
* pid_schedule_active_band_idx() — index (0-based) of the lower bracket
|
||
* entry used in the most recent interpolation. Useful for telemetry.
|
||
* Returns 0 if speed is below the first entry.
|
||
*/
|
||
uint8_t pid_schedule_active_band_idx(void);
|
||
|
||
/*
|
||
* pid_schedule_get_default_table(out_entries, out_n) — fill the 3-band
|
||
* default table into caller's buffer. Used for factory-reset.
|
||
*/
|
||
void pid_schedule_get_default_table(pid_sched_entry_t *out_entries, uint8_t *out_n);
|
||
|
||
#endif /* PID_SCHEDULE_H */
|