sl-controls
64de6667b6
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>
102 lines
3.8 KiB
C
102 lines
3.8 KiB
C
#ifndef PID_FLASH_H
|
|
#define PID_FLASH_H
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
|
|
/*
|
|
* pid_flash -- persistent PID storage for Issue #531 (auto-tune).
|
|
*
|
|
* Stores Kp, Ki, Kd in the last 64 bytes of STM32F722 flash sector 7
|
|
* (0x0807FFC0). Magic word validates presence of saved params.
|
|
* Sector 7 is 128KB starting at 0x08060000; firmware never exceeds sector 6.
|
|
*
|
|
* Flash writes require an erase of the full sector (128KB) before re-writing.
|
|
* The store address is the very last 64-byte block so future expansion can
|
|
* grow toward lower addresses within sector 7 without conflict.
|
|
*
|
|
* Issue #550 adds pid_sched_flash_t at 0x0807FF40 (128 bytes below the PID
|
|
* record). Both are written atomically by pid_flash_save_all() in one erase.
|
|
*
|
|
* Sector 7 layout:
|
|
* 0x0807FF40 pid_sched_flash_t (128 bytes) -- schedule (Issue #550)
|
|
* 0x0807FFC0 pid_flash_t ( 64 bytes) -- single PID (Issue #531)
|
|
* 0x08080000 sector boundary
|
|
*/
|
|
|
|
#define PID_FLASH_SECTOR FLASH_SECTOR_7
|
|
#define PID_FLASH_SECTOR_VOLTAGE VOLTAGE_RANGE_3 /* 2.7V-3.6V, 32-bit parallelism */
|
|
|
|
/* Sector 7: 128KB at 0x08060000; store single-PID in last 64 bytes */
|
|
#define PID_FLASH_STORE_ADDR 0x0807FFC0UL
|
|
#define PID_FLASH_MAGIC 0x534C5401UL /* 'SLT\x01' -- version 1 */
|
|
|
|
typedef struct __attribute__((packed)) {
|
|
uint32_t magic; /* PID_FLASH_MAGIC when valid */
|
|
float kp;
|
|
float ki;
|
|
float kd;
|
|
uint8_t _pad[48]; /* padding to 64 bytes */
|
|
} pid_flash_t;
|
|
|
|
/*
|
|
* pid_flash_load() -- read saved PID from flash.
|
|
* Returns true and fills *kp/*ki/*kd if magic is valid.
|
|
* Returns false if no valid params stored (caller keeps defaults).
|
|
*/
|
|
bool pid_flash_load(float *kp, float *ki, float *kd);
|
|
|
|
/*
|
|
* pid_flash_save() -- erase sector 7 and write Kp/Ki/Kd.
|
|
* Must not be called while armed (flash erase takes ~1s and stalls the CPU).
|
|
* Returns true on success.
|
|
*/
|
|
bool pid_flash_save(float kp, float ki, float kd);
|
|
|
|
/* ==================================================================
|
|
* PID Gain Schedule flash storage (Issue #550)
|
|
* ================================================================== */
|
|
|
|
#define PID_SCHED_MAX_BANDS 6u
|
|
#define PID_SCHED_FLASH_ADDR 0x0807FF40UL
|
|
#define PID_SCHED_MAGIC 0x534C5402UL /* 'SLT\x02' -- version 2 */
|
|
|
|
/* One speed band: 16 bytes, IEEE-754 LE floats */
|
|
typedef struct __attribute__((packed)) {
|
|
float speed_mps; /* breakpoint velocity (m/s, absolute) */
|
|
float kp;
|
|
float ki;
|
|
float kd;
|
|
} pid_sched_entry_t; /* 16 bytes */
|
|
|
|
/* Flash record: 128 bytes */
|
|
typedef struct __attribute__((packed)) {
|
|
uint32_t magic; /* 4 bytes */
|
|
uint8_t num_bands; /* 1 byte */
|
|
uint8_t flags; /* 1 byte (reserved) */
|
|
uint8_t _pad0[2]; /* 2 bytes */
|
|
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* 96 bytes */
|
|
uint8_t _pad1[24]; /* 24 bytes */
|
|
} pid_sched_flash_t; /* 128 bytes total */
|
|
|
|
/*
|
|
* pid_flash_load_schedule() -- read gain schedule from flash.
|
|
* Returns true and fills out_entries/out_n if magic valid and num_bands in
|
|
* [1, PID_SCHED_MAX_BANDS]. Returns false otherwise.
|
|
* out_entries must have room for PID_SCHED_MAX_BANDS entries.
|
|
*/
|
|
bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n);
|
|
|
|
/*
|
|
* pid_flash_save_all() -- erase sector 7 ONCE and write both records:
|
|
* - pid_sched_flash_t at PID_SCHED_FLASH_ADDR
|
|
* - pid_flash_t at PID_FLASH_STORE_ADDR
|
|
* Atomic: one sector erase covers both.
|
|
* Must not be called while armed (erase takes ~1s).
|
|
* Returns true on success.
|
|
*/
|
|
bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
|
|
const pid_sched_entry_t *entries, uint8_t num_bands);
|
|
|
|
#endif /* PID_FLASH_H */
|