sl-controls
2b06161cb4
Adds ADC-based motor current sensing with configurable overload threshold, soft PWM limiting, hard cutoff on sustained overload, and auto-recovery. Changes: - include/motor_current.h: MotorCurrentState enum (NORMAL/SOFT_LIMIT/COOLDOWN), thresholds (5A hard, 4A soft, 2s overload, 10s cooldown), full API - src/motor_current.c: reads battery_adc_get_current_ma() each tick (reuses existing ADC3 IN13/PC3 DMA sampling); linear PWM scale in soft-limit zone (scale256 fixed-point); fault counter + one-tick fault_pending flag for main-loop fault log integration; telemetry at MOTOR_CURR_TLM_HZ (5 Hz) - include/pid_flash.h: add pid_sched_entry_t (16 bytes), pid_sched_flash_t (128 bytes at 0x0807FF40), PID_SCHED_MAX_BANDS=6, pid_flash_load_schedule(), pid_flash_save_all() — fixes missing types needed by jlink.h (Issue #550) - src/pid_flash.c: implement flash_write_words() helper, pid_flash_load_schedule(), pid_flash_save_all() — single sector-7 erase covers both schedule and PID records - include/jlink.h: add JLINK_TLM_MOTOR_CURRENT (0x86), jlink_tlm_motor_current_t (8 bytes: current_ma, limit_pct, state, fault_count), jlink_send_motor_current_tlm() - src/jlink.c: implement jlink_send_motor_current_tlm() (14-byte frame) Motor overload state machine: MC_NORMAL : current_ma < 4000 mA — full PWM authority MC_SOFT_LIMIT : 4000-5000 mA — linear reduction (0% at 4A → 100% at 5A) MC_COOLDOWN : >5A sustained 2s → zero output for 10s then NORMAL Main-loop integration: motor_current_tick(now_ms); if (motor_current_fault_pending()) fault_log_append(FAULT_MOTOR_OVERCURRENT); cmd = motor_current_apply_limit(balance_pid_output()); motor_current_send_tlm(now_ms); Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
96 lines
3.6 KiB
C
96 lines
3.6 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.
|
||
*/
|
||
|
||
#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 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;
|
||
|
||
/* ---- Gain schedule flash storage (Issue #550) ---- */
|
||
|
||
/* Maximum number of speed-band entries in the gain schedule table */
|
||
#define PID_SCHED_MAX_BANDS 6u
|
||
|
||
/*
|
||
* Sector 7 layout (128KB at 0x08060000):
|
||
* 0x0807FF40 pid_sched_flash_t (128 bytes) — gain schedule record
|
||
* 0x0807FFC0 pid_flash_t ( 64 bytes) — single PID record (existing)
|
||
* Both records are written in a single sector erase via pid_flash_save_all().
|
||
*/
|
||
#define PID_SCHED_FLASH_ADDR 0x0807FF40UL
|
||
#define PID_SCHED_MAGIC 0x534C5402UL /* 'SLT\x02' — version 2 */
|
||
|
||
typedef struct __attribute__((packed)) {
|
||
float speed_mps; /* velocity breakpoint (m/s) */
|
||
float kp;
|
||
float ki;
|
||
float kd;
|
||
} pid_sched_entry_t; /* 16 bytes */
|
||
|
||
typedef struct __attribute__((packed)) {
|
||
uint32_t magic; /* PID_SCHED_MAGIC when valid */
|
||
uint8_t num_bands; /* valid entries (1..PID_SCHED_MAX_BANDS) */
|
||
uint8_t flags; /* reserved, must be 0 */
|
||
uint8_t _pad0[2];
|
||
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* 6 × 16 = 96 bytes */
|
||
uint8_t _pad1[24]; /* total = 4+1+1+2+96+24 = 128 bytes */
|
||
} pid_sched_flash_t; /* 128 bytes */
|
||
|
||
/*
|
||
* 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 (single-PID only).
|
||
* Use pid_flash_save_all() to save both single-PID and schedule atomically.
|
||
* 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_flash_load_schedule() — read gain schedule from flash.
|
||
* Returns true and fills out_entries[0..n-1] and *out_n if magic is valid.
|
||
* Returns false if no valid schedule stored.
|
||
*/
|
||
bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n);
|
||
|
||
/*
|
||
* pid_flash_save_all() — erase sector 7 once and atomically write both:
|
||
* - pid_sched_flash_t at PID_SCHED_FLASH_ADDR (0x0807FF40)
|
||
* - pid_flash_t at PID_FLASH_STORE_ADDR (0x0807FFC0)
|
||
* Must not be called while armed. 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 */
|