sl-controls
8592361095
Implements a speed-dependent PID gain scheduler that interpolates Kp/Ki/Kd across a configurable table of velocity breakpoints, replacing the fixed single-gain PID used previously. Changes: - include/pid_flash.h: add pid_sched_entry_t (16-byte entry), pid_sched_flash_t (128-byte record at 0x0807FF40), pid_flash_load_schedule(), pid_flash_save_all() (atomic single-sector erase for both schedule and single-PID records) - src/pid_flash.c: implement load_schedule and save_all; single erase covers both records at 0x0807FF40 (schedule) and 0x0807FFC0 (single PID) - include/pid_schedule.h: API header -- init, get_gains, apply, set/get table, flash_save, active_band_idx, get_default_table - src/pid_schedule.c: linear interpolation between sorted speed-band entries; integrator reset on band transition; default 3-band table (0/0.3/0.8 m/s) - include/jlink.h: add SCHED_GET (0x0C), SCHED_SET (0x0D), SCHED_SAVE (0x0E) commands; TLM_SCHED (0x85); jlink_tlm_sched_t; JLinkSchedSetBuf; sched_get_req, sched_save_req fields in JLinkState; include pid_flash.h - src/jlink.c: dispatch SCHED_GET/SET/SAVE; implement jlink_send_sched_telemetry, jlink_get_sched_set; add JLinkSchedSetBuf static buffer - test/test_pid_schedule.c: 48 unit tests -- all passing (gcc host build) Flash layout (sector 7): 0x0807FF40 pid_sched_flash_t (128 bytes) -- schedule 0x0807FFC0 pid_flash_t ( 64 bytes) -- single PID (existing) 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 */
|