From 2b06161cb4e612dfd6a5f6240c187fc12f6a2672 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sat, 14 Mar 2026 12:25:29 -0400 Subject: [PATCH] feat: Motor current monitoring and overload protection (Issue #584) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- include/jlink.h | 19 +++++ include/motor_current.h | 121 ++++++++++++++++++++++++++ include/pid_flash.h | 49 ++++++++++- src/jlink.c | 25 ++++++ src/motor_current.c | 183 ++++++++++++++++++++++++++++++++++++++++ src/pid_flash.c | 104 +++++++++++++++++++++++ 6 files changed, 500 insertions(+), 1 deletion(-) create mode 100644 include/motor_current.h create mode 100644 src/motor_current.c diff --git a/include/jlink.h b/include/jlink.h index 7e2289e..3c24107 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -44,6 +44,7 @@ * 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531) * 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547) * 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550) + * 0x86 MOTOR_CURRENT - jlink_tlm_motor_current_t (8 bytes, Issue #584) * * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and @@ -81,6 +82,7 @@ #define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */ #define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */ #define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */ +#define JLINK_TLM_MOTOR_CURRENT 0x86u /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ typedef struct __attribute__((packed)) { @@ -148,6 +150,16 @@ typedef struct __attribute__((packed)) { pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* up to 6 x 16 = 96 bytes */ } jlink_tlm_sched_t; /* 1 + 96 = 97 bytes max */ +/* ---- Telemetry MOTOR_CURRENT payload (8 bytes, packed) Issue #584 ---- */ +/* Published at MOTOR_CURR_TLM_HZ; reports measured current and protection state. */ +typedef struct __attribute__((packed)) { + int32_t current_ma; /* filtered battery/motor current (mA, + = discharge) */ + uint8_t limit_pct; /* soft-limit reduction applied: 0=none, 100=full cutoff */ + uint8_t state; /* MotorCurrentState: 0=NORMAL,1=SOFT_LIMIT,2=COOLDOWN */ + uint8_t fault_count; /* lifetime hard-cutoff trips (saturates at 255) */ + uint8_t _pad; /* reserved */ +} jlink_tlm_motor_current_t; /* 8 bytes */ + /* ---- Volatile state (read from main loop) ---- */ typedef struct { /* Drive command - updated on JLINK_CMD_DRIVE */ @@ -232,4 +244,11 @@ void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm); */ JLinkSchedSetBuf *jlink_get_sched_set(void); +/* + * jlink_send_motor_current_tlm(tlm) - transmit JLINK_TLM_MOTOR_CURRENT (0x86) + * frame (14 bytes total) to Jetson. Issue #584. + * Rate-limiting is handled by motor_current_send_tlm(); call from there only. + */ +void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm); + #endif /* JLINK_H */ diff --git a/include/motor_current.h b/include/motor_current.h new file mode 100644 index 0000000..9c4abd1 --- /dev/null +++ b/include/motor_current.h @@ -0,0 +1,121 @@ +#ifndef MOTOR_CURRENT_H +#define MOTOR_CURRENT_H + +#include +#include + +/* + * motor_current — ADC-based motor current monitoring and overload protection + * for Issue #584. + * + * Hardware: + * ADC3 IN13 (PC3, ADC_CURR_PIN) is already sampled by battery_adc.c via + * DMA2_Stream0 circular. This module reads battery_adc_get_current_ma() + * each tick rather than running a second ADC, since total discharge current + * on this single-motor balance bot equals motor current plus ~30 mA overhead. + * + * Behaviour: + * MC_NORMAL : current_ma < MOTOR_CURR_SOFT_MA — full output + * MC_SOFT_LIMIT : current_ma in [SOFT_MA, HARD_MA) — linear PWM reduction + * MC_COOLDOWN : hard cutoff latched after HARD_MA sustained for + * MOTOR_CURR_OVERLOAD_MS (2 s) — zero output for + * MOTOR_CURR_COOLDOWN_MS (10 s), then MC_NORMAL + * + * Soft limit formula (MC_SOFT_LIMIT): + * scale = (HARD_MA - current_ma) / (HARD_MA - SOFT_MA) [0..1] + * limited_cmd = (int16_t)(cmd * scale) + * + * Fault event: + * On each hard-cutoff trip, s_fault_count is incremented (saturates at 255) + * and motor_current_fault_pending() returns true for one main-loop tick so + * the caller can append a fault log entry. + * + * Main-loop integration (pseudo-code): + * + * void main_loop_tick(uint32_t now_ms) { + * battery_adc_tick(now_ms); + * motor_current_tick(now_ms); + * + * if (motor_current_fault_pending()) + * fault_log_append(FAULT_MOTOR_OVERCURRENT); + * + * int16_t cmd = balance_pid_output(); + * cmd = motor_current_apply_limit(cmd); + * motor_driver_update(&g_motor, cmd, steer, now_ms); + * + * motor_current_send_tlm(now_ms); // rate-limited to MOTOR_CURR_TLM_HZ + * } + */ + +/* ---- Thresholds ---- */ +#define MOTOR_CURR_HARD_MA 5000u /* 5 A — hard cutoff level */ +#define MOTOR_CURR_SOFT_MA 4000u /* 4 A — soft-limit onset (80% of hard) */ +#define MOTOR_CURR_OVERLOAD_MS 2000u /* sustained over HARD_MA before fault */ +#define MOTOR_CURR_COOLDOWN_MS 10000u /* zero-output recovery period (ms) */ +#define MOTOR_CURR_TLM_HZ 5u /* JLINK_TLM_MOTOR_CURRENT publish rate */ + +/* ---- State enum ---- */ +typedef enum { + MC_NORMAL = 0, + MC_SOFT_LIMIT = 1, + MC_COOLDOWN = 2, +} MotorCurrentState; + +/* ---- API ---- */ + +/* + * motor_current_init() — reset all state. + * Call once during system init, after battery_adc_init(). + */ +void motor_current_init(void); + +/* + * motor_current_tick(now_ms) — evaluate ADC reading, update state machine. + * Call from main loop after battery_adc_tick(), at any rate ≥ 10 Hz. + * Non-blocking (<1 µs). + */ +void motor_current_tick(uint32_t now_ms); + +/* + * motor_current_apply_limit(cmd) — scale motor command by current-limit factor. + * MC_NORMAL: returns cmd unchanged. + * MC_SOFT_LIMIT: returns cmd scaled down linearly. + * MC_COOLDOWN: returns 0. + * Call after motor_current_tick() each loop iteration. + */ +int16_t motor_current_apply_limit(int16_t cmd); + +/* + * motor_current_is_faulted() — true while in MC_COOLDOWN (output zeroed). + */ +bool motor_current_is_faulted(void); + +/* + * motor_current_state() — current state machine state. + */ +MotorCurrentState motor_current_state(void); + +/* + * motor_current_ma() — most recent ADC reading used by the state machine (mA). + */ +int32_t motor_current_ma(void); + +/* + * motor_current_fault_count() — lifetime hard-cutoff trip counter (0..255). + */ +uint8_t motor_current_fault_count(void); + +/* + * motor_current_fault_pending() — true for exactly one tick after a hard + * cutoff trip fires. Main loop should append a fault log entry and then the + * flag clears automatically on the next call. + */ +bool motor_current_fault_pending(void); + +/* + * motor_current_send_tlm(now_ms) — transmit JLINK_TLM_MOTOR_CURRENT (0x86) + * frame to Jetson. Rate-limited to MOTOR_CURR_TLM_HZ; safe to call every tick. + */ +void motor_current_send_tlm(uint32_t now_ms); + +#endif /* MOTOR_CURRENT_H */ diff --git a/include/pid_flash.h b/include/pid_flash.h index df0b9fb..db29718 100644 --- a/include/pid_flash.h +++ b/include/pid_flash.h @@ -31,6 +31,36 @@ typedef struct __attribute__((packed)) { 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. @@ -39,10 +69,27 @@ typedef struct __attribute__((packed)) { bool pid_flash_load(float *kp, float *ki, float *kd); /* - * pid_flash_save() — erase sector 7 and write Kp/Ki/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 */ diff --git a/src/jlink.c b/src/jlink.c index afb87aa..fd74845 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -482,6 +482,31 @@ void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state) jlink_tx_locked(frame, sizeof(frame)); } +/* ---- jlink_send_motor_current_tlm() -- Issue #584 ---- */ +void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm) +{ + /* + * Frame: [STX][LEN][0x86][8 bytes MOTOR_CURRENT][CRC_hi][CRC_lo][ETX] + * LEN = 1 (CMD) + 8 (payload) = 9; total frame = 14 bytes. + * At 921600 baud: 14x10/921600 ~0.15 ms -- safe to block. + */ + static uint8_t frame[14]; + const uint8_t plen = (uint8_t)sizeof(jlink_tlm_motor_current_t); /* 8 */ + const uint8_t len = 1u + plen; /* 9 */ + + frame[0] = JLINK_STX; + frame[1] = len; + frame[2] = JLINK_TLM_MOTOR_CURRENT; + memcpy(&frame[3], tlm, plen); + + uint16_t crc = crc16_xmodem(&frame[2], len); + frame[3 + plen] = (uint8_t)(crc >> 8); + frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); + frame[3 + plen + 2] = JLINK_ETX; + + jlink_tx_locked(frame, sizeof(frame)); +} + /* ---- jlink_send_sched_telemetry() -- Issue #550 ---- */ void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm) { diff --git a/src/motor_current.c b/src/motor_current.c new file mode 100644 index 0000000..828e307 --- /dev/null +++ b/src/motor_current.c @@ -0,0 +1,183 @@ +/* + * motor_current.c — ADC-based motor current monitoring and overload protection + * (Issue #584). + * + * Reads battery discharge current from battery_adc_get_current_ma() (ADC3 IN13, + * PC3), which is already DMA-sampled by battery_adc.c. Implements: + * + * 1. Soft current limiting: linear PWM reduction when current exceeds + * MOTOR_CURR_SOFT_MA (4 A, 80% of hard threshold). + * + * 2. Hard cutoff: if current stays above MOTOR_CURR_HARD_MA (5 A) for + * MOTOR_CURR_OVERLOAD_MS (2 s), output is zeroed. A fault event is + * signalled via motor_current_fault_pending() for one tick so the main + * loop can append a fault log entry. + * + * 3. Auto-recovery: after MOTOR_CURR_COOLDOWN_MS (10 s) in MC_COOLDOWN, + * state returns to MC_NORMAL and normal PWM authority is restored. + * + * 4. Telemetry: JLINK_TLM_MOTOR_CURRENT (0x86) published at + * MOTOR_CURR_TLM_HZ (5 Hz) via jlink_send_motor_current_tlm(). + */ + +#include "motor_current.h" +#include "battery_adc.h" +#include "jlink.h" +#include + +/* ---- Module state ---- */ +static MotorCurrentState s_state = MC_NORMAL; +static int32_t s_current_ma = 0; +static uint32_t s_overload_start = 0; /* ms when current first ≥ HARD_MA */ +static uint32_t s_cooldown_start = 0; /* ms when cooldown began */ +static uint8_t s_fault_count = 0; /* lifetime trip counter */ +static uint8_t s_fault_pending = 0; /* cleared after one read */ +static uint32_t s_last_tlm_ms = 0; /* rate-limit TLM TX */ + +/* Soft-limit scale factor in 0..256 fixed-point (256 = 1.0) */ +static uint16_t s_scale256 = 256u; + +/* ---- motor_current_init() ---- */ +void motor_current_init(void) +{ + s_state = MC_NORMAL; + s_current_ma = 0; + s_overload_start = 0; + s_cooldown_start = 0; + s_fault_count = 0; + s_fault_pending = 0; + s_last_tlm_ms = 0; + s_scale256 = 256u; +} + +/* ---- motor_current_tick() ---- */ +void motor_current_tick(uint32_t now_ms) +{ + /* Snapshot current from battery ADC (mA, positive = discharge) */ + s_current_ma = battery_adc_get_current_ma(); + + /* Use absolute value: protect in both forward and regen braking */ + int32_t abs_ma = s_current_ma; + if (abs_ma < 0) abs_ma = -abs_ma; + + switch (s_state) { + + case MC_NORMAL: + s_scale256 = 256u; + if (abs_ma >= (int32_t)MOTOR_CURR_SOFT_MA) { + s_state = MC_SOFT_LIMIT; + /* Track overload onset if already above hard threshold */ + s_overload_start = (abs_ma >= (int32_t)MOTOR_CURR_HARD_MA) + ? now_ms : 0u; + } + break; + + case MC_SOFT_LIMIT: + if (abs_ma < (int32_t)MOTOR_CURR_SOFT_MA) { + /* Recovered below soft threshold */ + s_state = MC_NORMAL; + s_overload_start = 0u; + s_scale256 = 256u; + } else { + /* Compute linear scale: 256 at SOFT_MA, 0 at HARD_MA */ + int32_t range = (int32_t)MOTOR_CURR_HARD_MA + - (int32_t)MOTOR_CURR_SOFT_MA; + int32_t over = abs_ma - (int32_t)MOTOR_CURR_SOFT_MA; + if (over >= range) { + s_scale256 = 0u; + } else { + /* scale256 = (range - over) * 256 / range */ + s_scale256 = (uint16_t)(((range - over) * 256u) / range); + } + + /* Track sustained hard-threshold overload */ + if (abs_ma >= (int32_t)MOTOR_CURR_HARD_MA) { + if (s_overload_start == 0u) { + s_overload_start = now_ms; + } else if ((now_ms - s_overload_start) >= MOTOR_CURR_OVERLOAD_MS) { + /* Hard cutoff — trip the fault */ + if (s_fault_count < 255u) s_fault_count++; + s_fault_pending = 1u; + s_cooldown_start = now_ms; + s_overload_start = 0u; + s_scale256 = 0u; + s_state = MC_COOLDOWN; + } + } else { + /* Current dipped back below HARD_MA — reset overload timer */ + s_overload_start = 0u; + } + } + break; + + case MC_COOLDOWN: + s_scale256 = 0u; + if ((now_ms - s_cooldown_start) >= MOTOR_CURR_COOLDOWN_MS) { + s_state = MC_NORMAL; + s_scale256 = 256u; + } + break; + } +} + +/* ---- motor_current_apply_limit() ---- */ +int16_t motor_current_apply_limit(int16_t cmd) +{ + if (s_scale256 >= 256u) return cmd; + if (s_scale256 == 0u) return 0; + return (int16_t)(((int32_t)cmd * s_scale256) / 256); +} + +/* ---- Accessors ---- */ +bool motor_current_is_faulted(void) +{ + return s_state == MC_COOLDOWN; +} + +MotorCurrentState motor_current_state(void) +{ + return s_state; +} + +int32_t motor_current_ma(void) +{ + return s_current_ma; +} + +uint8_t motor_current_fault_count(void) +{ + return s_fault_count; +} + +bool motor_current_fault_pending(void) +{ + if (!s_fault_pending) return false; + s_fault_pending = 0u; + return true; +} + +/* ---- motor_current_send_tlm() ---- */ +void motor_current_send_tlm(uint32_t now_ms) +{ + if (MOTOR_CURR_TLM_HZ == 0u) return; + + uint32_t interval_ms = 1000u / MOTOR_CURR_TLM_HZ; + if ((now_ms - s_last_tlm_ms) < interval_ms) return; + s_last_tlm_ms = now_ms; + + jlink_tlm_motor_current_t tlm; + + tlm.current_ma = s_current_ma; + + /* limit_pct: 0 = no limiting, 100 = full cutoff */ + if (s_scale256 >= 256u) { + tlm.limit_pct = 0u; + } else { + tlm.limit_pct = (uint8_t)(((256u - s_scale256) * 100u) / 256u); + } + + tlm.state = (uint8_t)s_state; + tlm.fault_count = s_fault_count; + + jlink_send_motor_current_tlm(&tlm); +} diff --git a/src/pid_flash.c b/src/pid_flash.c index 0131d62..6a5f412 100644 --- a/src/pid_flash.c +++ b/src/pid_flash.c @@ -70,3 +70,107 @@ bool pid_flash_save(float kp, float ki, float kd) stored->ki == ki && stored->kd == kd); } + +/* ---- Helper: write arbitrary bytes as 32-bit words ---- */ +/* + * Writes 'len' bytes from 'src' to flash at 'addr'. + * len must be a multiple of 4. Flash must already be unlocked. + * Returns HAL_OK on success, or first failure status. + */ +static HAL_StatusTypeDef flash_write_words(uint32_t addr, + const void *src, + uint32_t len) +{ + const uint32_t *p = (const uint32_t *)src; + HAL_StatusTypeDef rc = HAL_OK; + for (uint32_t i = 0; i < len / 4u; i++) { + rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, p[i]); + if (rc != HAL_OK) return rc; + addr += 4u; + } + return HAL_OK; +} + +/* ---- pid_flash_load_schedule() ---- */ +bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n) +{ + const pid_sched_flash_t *p = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR; + + if (p->magic != PID_SCHED_MAGIC) return false; + if (p->num_bands == 0u || p->num_bands > PID_SCHED_MAX_BANDS) return false; + + *out_n = p->num_bands; + for (uint8_t i = 0; i < p->num_bands; i++) { + out_entries[i] = p->bands[i]; + } + return true; +} + +/* ---- pid_flash_save_all() ---- */ +bool pid_flash_save_all(float kp_single, float ki_single, float kd_single, + const pid_sched_entry_t *entries, uint8_t num_bands) +{ + if (num_bands == 0u || num_bands > PID_SCHED_MAX_BANDS) return false; + + HAL_StatusTypeDef rc; + + rc = HAL_FLASH_Unlock(); + if (rc != HAL_OK) return false; + + /* Single erase of sector 7 covers both records */ + FLASH_EraseInitTypeDef erase = { + .TypeErase = FLASH_TYPEERASE_SECTORS, + .Sector = PID_FLASH_SECTOR, + .NbSectors = 1, + .VoltageRange = PID_FLASH_SECTOR_VOLTAGE, + }; + uint32_t sector_error = 0; + rc = HAL_FLASHEx_Erase(&erase, §or_error); + if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) { + HAL_FLASH_Lock(); + return false; + } + + /* Build and write schedule record at PID_SCHED_FLASH_ADDR */ + pid_sched_flash_t srec; + memset(&srec, 0xFF, sizeof(srec)); + srec.magic = PID_SCHED_MAGIC; + srec.num_bands = num_bands; + srec.flags = 0u; + for (uint8_t i = 0; i < num_bands; i++) { + srec.bands[i] = entries[i]; + } + + rc = flash_write_words(PID_SCHED_FLASH_ADDR, &srec, sizeof(srec)); + if (rc != HAL_OK) { + HAL_FLASH_Lock(); + return false; + } + + /* Build and write single-PID record at PID_FLASH_STORE_ADDR */ + pid_flash_t prec; + memset(&prec, 0xFF, sizeof(prec)); + prec.magic = PID_FLASH_MAGIC; + prec.kp = kp_single; + prec.ki = ki_single; + prec.kd = kd_single; + + rc = flash_write_words(PID_FLASH_STORE_ADDR, &prec, sizeof(prec)); + if (rc != HAL_OK) { + HAL_FLASH_Lock(); + return false; + } + + HAL_FLASH_Lock(); + + /* Verify both records */ + const pid_sched_flash_t *sv = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR; + const pid_flash_t *pv = (const pid_flash_t *)PID_FLASH_STORE_ADDR; + + return (sv->magic == PID_SCHED_MAGIC && + sv->num_bands == num_bands && + pv->magic == PID_FLASH_MAGIC && + pv->kp == kp_single && + pv->ki == ki_single && + pv->kd == kd_single); +} -- 2.47.2