feat: PID gain scheduling for speed-dependent balance (Issue #550)

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>

include/jlink.h

@@ -3,9 +3,10 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#include "pid_flash.h"   /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */
 
 /*
- * JLink — Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX).
+ * JLink -- Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX).
  *
  * Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated
  * hardware UART at 921600 baud using DMA circular RX and IDLE interrupt.
@@ -28,12 +29,19 @@
  *   0x05  PID_SET    - float kp, float ki, float kd (12 bytes, IEEE-754 LE)
  *   0x06  DFU_ENTER  - no payload; request OTA DFU reboot (denied while armed)
  *   0x07  ESTOP      - no payload; engage emergency stop
+ *   0x08  AUDIO      - int16 PCM samples (up to 126 samples)
+ *   0x09  SLEEP      - no payload; request STOP-mode sleep
  *   0x0A  PID_SAVE   - no payload; save current Kp/Ki/Kd to flash (Issue #531)
+ *   0x0C  SCHED_GET  - no payload; reply with TLM_SCHED (Issue #550)
+ *   0x0D  SCHED_SET  - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550)
+ *   0x0E  SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550)
  *
  * STM32 to Jetson telemetry:
  *   0x80  STATUS     - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
  *   0x81  POWER      - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
+ *   0x82  BATTERY    - jlink_tlm_battery_t (10 bytes, Issue #533)
  *   0x83  PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
+ *   0x85  SCHED      - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
  *
  * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
  * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
@@ -59,12 +67,16 @@
 #define JLINK_CMD_AUDIO         0x08u  /* PCM audio chunk: int16 samples, up to 126 */
 #define JLINK_CMD_SLEEP         0x09u  /* no payload; request STOP-mode sleep */
 #define JLINK_CMD_PID_SAVE      0x0Au  /* no payload; save Kp/Ki/Kd to flash (Issue #531) */
+#define JLINK_CMD_SCHED_GET     0x0Cu  /* no payload; reply TLM_SCHED (Issue #550) */
+#define JLINK_CMD_SCHED_SET     0x0Du  /* uint8 num_bands + N*16-byte entries (Issue #550) */
+#define JLINK_CMD_SCHED_SAVE    0x0Eu  /* float kp,ki,kd; save sched+single to flash (Issue #550) */
 
 /* ---- Telemetry IDs (STM32 to Jetson) ---- */
 #define JLINK_TLM_STATUS        0x80u
 #define JLINK_TLM_POWER         0x81u  /* jlink_tlm_power_t (11 bytes) */
 #define JLINK_TLM_BATTERY       0x82u  /* jlink_tlm_battery_t (10 bytes, Issue #533) */
-#define JLINK_TLM_PID_RESULT    0x83u  /* jlink_tlm_pid_result_t (13 bytes) Issue #531 */
+#define JLINK_TLM_PID_RESULT    0x83u  /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */
+#define JLINK_TLM_SCHED         0x85u  /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
 typedef struct __attribute__((packed)) {
@@ -94,15 +106,15 @@ typedef struct __attribute__((packed)) {
     uint32_t idle_ms;       /* ms since last cmd_vel activity */
 } jlink_tlm_power_t;  /* 11 bytes */
 
-/* ---- Telemetry BATTERY payload (10 bytes, packed) — Issue #533 ---- */
+/* ---- Telemetry BATTERY payload (10 bytes, packed) Issue #533 ---- */
 typedef struct __attribute__((packed)) {
-    uint16_t vbat_mv;     /* DMA-sampled LPF-filtered Vbat (mV)               */
+    uint16_t vbat_mv;     /* DMA-sampled LPF-filtered Vbat (mV) */
     int16_t  ibat_ma;     /* DMA-sampled LPF-filtered Ibat (mA, + = discharge) */
-    uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV)                 */
-    uint8_t  flags;       /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready  */
-    int8_t   cal_offset;  /* vbat_offset_mv / 4 (±127 → ±508 mV)              */
-    uint8_t  lpf_shift;   /* IIR shift factor (α = 1/2^lpf_shift)              */
-    uint8_t  soc_pct;     /* voltage-based SoC 0–100, 255 = unknown            */
+    uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */
+    uint8_t  flags;       /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */
+    int8_t   cal_offset;  /* vbat_offset_mv / 4 (+-127 -> +-508 mV) */
+    uint8_t  lpf_shift;   /* IIR shift factor (alpha = 1/2^lpf_shift) */
+    uint8_t  soc_pct;     /* voltage-based SoC 0-100, 255 = unknown */
 } jlink_tlm_battery_t;  /* 10 bytes */
 
 /* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */
@@ -114,6 +126,13 @@ typedef struct __attribute__((packed)) {
     uint8_t saved_ok;  /* 1 = flash write verified, 0 = write failed */
 } jlink_tlm_pid_result_t;  /* 13 bytes */
 
+/* ---- Telemetry SCHED payload (1 + N*16 bytes, packed) Issue #550 ---- */
+/* Sent in response to JLINK_CMD_SCHED_GET; N = num_bands (1..PID_SCHED_MAX_BANDS). */
+typedef struct __attribute__((packed)) {
+    uint8_t           num_bands;                   /* number of valid entries */
+    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 */
+
 /* ---- Volatile state (read from main loop) ---- */
 typedef struct {
     /* Drive command - updated on JLINK_CMD_DRIVE */
@@ -140,54 +159,50 @@ typedef struct {
     volatile uint8_t  sleep_req;
     /* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */
     volatile uint8_t  pid_save_req;
+
+    /* PID schedule commands (Issue #550) - set by parser, cleared by main loop */
+    volatile uint8_t  sched_get_req;   /* SCHED_GET: main loop calls jlink_send_sched_telemetry() */
+    volatile uint8_t  sched_save_req;  /* SCHED_SAVE: main loop calls pid_schedule_flash_save() */
+    volatile float    sched_save_kp;   /* kp for single-PID record in SCHED_SAVE */
+    volatile float    sched_save_ki;
+    volatile float    sched_save_kd;
 } JLinkState;
 
 extern volatile JLinkState jlink_state;
 
+/* ---- SCHED_SET receive buffer -- Issue #550 ---- */
+/*
+ * Populated by the parser on JLINK_CMD_SCHED_SET.  Main loop reads via
+ * jlink_get_sched_set() and calls pid_schedule_set_table() before clearing.
+ */
+typedef struct {
+    volatile uint8_t   ready;                        /* set by parser, cleared by main loop */
+    volatile uint8_t   num_bands;
+    pid_sched_entry_t  bands[PID_SCHED_MAX_BANDS];   /* copied from frame */
+} JLinkSchedSetBuf;
+
 /* ---- API ---- */
 
-/*
- * jlink_init() - configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
- * DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt.
- * Call once before safety_init().
- */
 void jlink_init(void);
-
-/*
- * jlink_is_active(now_ms) - returns true if a valid frame arrived within
- * JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received.
- */
 bool jlink_is_active(uint32_t now_ms);
-
-/*
- * jlink_send_telemetry(status) - build and transmit a JLINK_TLM_STATUS frame
- * over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ.
- */
-void jlink_send_telemetry(const jlink_tlm_status_t *status);
-
-/*
- * jlink_process() - drain DMA circular buffer and parse frames.
- * Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending).
- */
 void jlink_process(void);
-
-/*
- * jlink_send_power_telemetry(power) - build and transmit a JLINK_TLM_POWER
- * frame (17 bytes) at PM_TLM_HZ. Call from main loop when not in STOP mode.
- */
+void jlink_send_telemetry(const jlink_tlm_status_t *status);
 void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
-
-/*
- * jlink_send_pid_result(result) - build and transmit a JLINK_TLM_PID_RESULT
- * frame (19 bytes) to confirm PID flash save outcome (Issue #531).
- */
+void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt);
 void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
 
 /*
- * jlink_send_battery_telemetry(batt) - build and transmit JLINK_TLM_BATTERY
- * (0x82) frame (16 bytes) at BATTERY_ADC_PUBLISH_HZ (1 Hz).
- * Called by battery_adc_publish(); not normally called directly.
+ * jlink_send_sched_telemetry(tlm) - transmit JLINK_TLM_SCHED (0x85) in
+ * response to SCHED_GET.  tlm->num_bands determines actual frame size.
+ * Issue #550.
  */
-void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt);
+void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm);
+
+/*
+ * jlink_get_sched_set() - return pointer to the most-recently received
+ * SCHED_SET payload buffer (static storage in jlink.c).  Main loop calls
+ * pid_schedule_set_table() from this buffer, then clears ready.  Issue #550.
+ */
+JLinkSchedSetBuf *jlink_get_sched_set(void);
 
 #endif /* JLINK_H */

include/pid_flash.h

@@ -5,7 +5,7 @@
 #include <stdbool.h>
 
 /*
- * pid_flash — persistent PID storage for Issue #531 (auto-tune).
+ * 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.
@@ -14,14 +14,22 @@
  * 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         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 */
+/* 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 */
+#define PID_FLASH_MAGIC         0x534C5401UL   /* 'SLT\x01' -- version 1 */
 
 typedef struct __attribute__((packed)) {
     uint32_t magic;       /* PID_FLASH_MAGIC when valid */
@@ -32,17 +40,62 @@ typedef struct __attribute__((packed)) {
 } pid_flash_t;
 
 /*
- * pid_flash_load() — read saved PID from flash.
+ * 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.
+ * 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 */

include/pid_schedule.h

@@ -0,0 +1,122 @@
+/*
+ * 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 */

src/pid_flash.c

@@ -8,7 +8,7 @@ bool pid_flash_load(float *kp, float *ki, float *kd)
 
     if (p->magic != PID_FLASH_MAGIC) return false;
 
-    /* Basic sanity bounds — same as JLINK_CMD_PID_SET handler */
+    /* Basic sanity bounds -- same as JLINK_CMD_PID_SET handler */
     if (p->kp < 0.0f || p->kp > 500.0f) return false;
     if (p->ki < 0.0f || p->ki > 50.0f)  return false;
     if (p->kd < 0.0f || p->kd > 50.0f)  return false;
@@ -49,7 +49,7 @@ bool pid_flash_save(float kp, float ki, float kd)
     rec.ki    = ki;
     rec.kd    = kd;
 
-    /* Write 64 bytes as 16 × 32-bit words */
+    /* Write 64 bytes as 16 x 32-bit words */
     const uint32_t *src  = (const uint32_t *)&rec;
     uint32_t        addr = PID_FLASH_STORE_ADDR;
     for (uint8_t i = 0; i < sizeof(rec) / 4u; i++) {
@@ -70,3 +70,92 @@ bool pid_flash_save(float kp, float ki, float kd)
             stored->ki == ki &&
             stored->kd == kd);
 }
+
+/* ---- Helper: write N bytes to flash as 32-bit words ---- */
+static bool flash_write_words(uint32_t addr, const void *src_buf, uint32_t byte_len)
+{
+    const uint32_t *w = (const uint32_t *)src_buf;
+    for (uint32_t i = 0; i < byte_len / 4u; i++) {
+        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, w[i]) != HAL_OK)
+            return false;
+        addr += 4u;
+    }
+    return true;
+}
+
+/* ---- 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;
+
+    memcpy(out_entries, p->bands, p->num_bands * sizeof(pid_sched_entry_t));
+    *out_n = p->num_bands;
+    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)
+{
+    HAL_StatusTypeDef rc;
+
+    if (num_bands == 0u || num_bands > PID_SCHED_MAX_BANDS) return false;
+
+    rc = HAL_FLASH_Unlock();
+    if (rc != HAL_OK) return false;
+
+    /* Erase sector 7 -- one erase 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 = 0u;
+    rc = HAL_FLASHEx_Erase(&erase, &sector_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;
+    srec._pad0[0]  = 0u;
+    srec._pad0[1]  = 0u;
+    memcpy(srec.bands, entries, num_bands * sizeof(pid_sched_entry_t));
+
+    if (!flash_write_words(PID_SCHED_FLASH_ADDR, &srec, sizeof(srec))) {
+        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;
+
+    if (!flash_write_words(PID_FLASH_STORE_ADDR, &prec, sizeof(prec))) {
+        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);
+}

src/pid_schedule.c

@@ -0,0 +1,174 @@
+#include "pid_schedule.h"
+#include "pid_flash.h"
+#include <string.h>
+#include <math.h>   /* fabsf */
+
+/* ---- Default 3-band table ---- */
+static const pid_sched_entry_t k_default_table[3] = {
+    { .speed_mps = 0.00f, .kp = 40.0f, .ki = 1.5f, .kd = 1.2f },
+    { .speed_mps = 0.30f, .kp = 35.0f, .ki = 1.0f, .kd = 1.0f },
+    { .speed_mps = 0.80f, .kp = 28.0f, .ki = 0.5f, .kd = 0.8f },
+};
+
+/* ---- Active table ---- */
+static pid_sched_entry_t s_bands[PID_SCHED_MAX_BANDS];
+static uint8_t           s_num_bands    = 0u;
+static uint8_t           s_active_band  = 0u;   /* lower-bracket index of last call */
+static uint8_t           s_prev_band    = 0xFFu; /* sentinel: forces integrator reset on first apply */
+
+/* ---- sort helper (insertion sort — table is small, ≤6 entries) ---- */
+static void sort_bands(void)
+{
+    for (uint8_t i = 1u; i < s_num_bands; i++) {
+        pid_sched_entry_t key = s_bands[i];
+        int8_t j = (int8_t)(i - 1u);
+        while (j >= 0 && s_bands[j].speed_mps > key.speed_mps) {
+            s_bands[j + 1] = s_bands[j];
+            j--;
+        }
+        s_bands[j + 1] = key;
+    }
+}
+
+/* ---- pid_schedule_init() ---- */
+void pid_schedule_init(void)
+{
+    pid_sched_entry_t tmp[PID_SCHED_MAX_BANDS];
+    uint8_t           n = 0u;
+
+    if (pid_flash_load_schedule(tmp, &n)) {
+        /* Validate entries minimally */
+        bool ok = true;
+        for (uint8_t i = 0u; i < n; i++) {
+            if (tmp[i].kp < 0.0f || tmp[i].kp > 500.0f ||
+                tmp[i].ki < 0.0f || tmp[i].ki > 50.0f  ||
+                tmp[i].kd < 0.0f || tmp[i].kd > 50.0f  ||
+                tmp[i].speed_mps < 0.0f) {
+                ok = false;
+                break;
+            }
+        }
+        if (ok) {
+            memcpy(s_bands, tmp, n * sizeof(pid_sched_entry_t));
+            s_num_bands = n;
+            sort_bands();
+            s_active_band = 0u;
+            return;
+        }
+    }
+
+    /* Fall back to built-in default */
+    memcpy(s_bands, k_default_table, sizeof(k_default_table));
+    s_num_bands   = 3u;
+    s_active_band = 0u;
+    s_prev_band   = 0xFFu;
+}
+
+/* ---- pid_schedule_get_gains() ---- */
+void pid_schedule_get_gains(float speed_mps, float *kp, float *ki, float *kd)
+{
+    float spd = fabsf(speed_mps);
+
+    if (s_num_bands == 0u) {
+        *kp = k_default_table[0].kp;
+        *ki = k_default_table[0].ki;
+        *kd = k_default_table[0].kd;
+        return;
+    }
+
+    /* Clamp below first entry */
+    if (spd <= s_bands[0].speed_mps) {
+        s_active_band = 0u;
+        *kp = s_bands[0].kp;
+        *ki = s_bands[0].ki;
+        *kd = s_bands[0].kd;
+        return;
+    }
+
+    /* Clamp above last entry */
+    uint8_t last = s_num_bands - 1u;
+    if (spd >= s_bands[last].speed_mps) {
+        s_active_band = last;
+        *kp = s_bands[last].kp;
+        *ki = s_bands[last].ki;
+        *kd = s_bands[last].kd;
+        return;
+    }
+
+    /* Find bracket [i-1, i] where bands[i-1].speed <= spd < bands[i].speed */
+    uint8_t i = 1u;
+    while (i < s_num_bands && s_bands[i].speed_mps <= spd) i++;
+    /* Now bands[i-1].speed_mps <= spd < bands[i].speed_mps */
+
+    s_active_band = (uint8_t)(i - 1u);
+
+    float dv = s_bands[i].speed_mps - s_bands[i - 1u].speed_mps;
+    float t  = (dv > 0.0f) ? (spd - s_bands[i - 1u].speed_mps) / dv : 0.0f;
+
+    *kp = s_bands[i - 1u].kp + t * (s_bands[i].kp - s_bands[i - 1u].kp);
+    *ki = s_bands[i - 1u].ki + t * (s_bands[i].ki - s_bands[i - 1u].ki);
+    *kd = s_bands[i - 1u].kd + t * (s_bands[i].kd - s_bands[i - 1u].kd);
+}
+
+/* ---- pid_schedule_apply() ---- */
+void pid_schedule_apply(balance_t *b, float speed_mps)
+{
+    float kp, ki, kd;
+    pid_schedule_get_gains(speed_mps, &kp, &ki, &kd);
+
+    b->kp = kp;
+    b->ki = ki;
+    b->kd = kd;
+
+    /* Reset integrator on band transition to prevent windup spike */
+    if (s_active_band != s_prev_band) {
+        b->integral   = 0.0f;
+        s_prev_band   = s_active_band;
+    }
+}
+
+/* ---- pid_schedule_set_table() ---- */
+void pid_schedule_set_table(const pid_sched_entry_t *entries, uint8_t n)
+{
+    if (n == 0u) n = 1u;
+    if (n > PID_SCHED_MAX_BANDS) n = PID_SCHED_MAX_BANDS;
+
+    memcpy(s_bands, entries, n * sizeof(pid_sched_entry_t));
+    s_num_bands   = n;
+    s_active_band = 0u;
+    s_prev_band   = 0xFFu;
+    sort_bands();
+}
+
+/* ---- pid_schedule_get_table() ---- */
+void pid_schedule_get_table(pid_sched_entry_t *out_entries, uint8_t *out_n)
+{
+    memcpy(out_entries, s_bands, s_num_bands * sizeof(pid_sched_entry_t));
+    *out_n = s_num_bands;
+}
+
+/* ---- pid_schedule_get_num_bands() ---- */
+uint8_t pid_schedule_get_num_bands(void)
+{
+    return s_num_bands;
+}
+
+/* ---- pid_schedule_flash_save() ---- */
+bool pid_schedule_flash_save(float kp_single, float ki_single, float kd_single)
+{
+    return pid_flash_save_all(kp_single, ki_single, kd_single,
+                              s_bands, s_num_bands);
+}
+
+/* ---- pid_schedule_active_band_idx() ---- */
+uint8_t pid_schedule_active_band_idx(void)
+{
+    return s_active_band;
+}
+
+/* ---- pid_schedule_get_default_table() ---- */
+void pid_schedule_get_default_table(pid_sched_entry_t *out_entries, uint8_t *out_n)
+{
+    memcpy(out_entries, k_default_table, sizeof(k_default_table));
+    *out_n = 3u;
+}

test/test_pid_schedule.c

@@ -0,0 +1,441 @@
+/*
+ * test_pid_schedule.c -- host-side unit tests for pid_schedule (Issue #550)
+ *
+ * Build:
+ *   gcc -I /tmp/stub_hal -I include -DTEST_HOST -lm \
+ *       -o test_pid_schedule test/test_pid_schedule.c
+ *
+ * Run:
+ *   ./test_pid_schedule
+ */
+
+/* ---- Minimal HAL stub (no hardware) ---- */
+#ifndef STM32F7XX_HAL_H
+#define STM32F7XX_HAL_H
+#include <stdint.h>
+#include <stdbool.h>
+typedef enum { HAL_OK = 0 } HAL_StatusTypeDef;
+typedef struct { uint32_t TypeErase; uint32_t Sector; uint32_t NbSectors; uint32_t VoltageRange; } FLASH_EraseInitTypeDef;
+#define FLASH_TYPEERASE_SECTORS 0
+#define FLASH_SECTOR_7          7
+#define VOLTAGE_RANGE_3         3
+#define FLASH_TYPEPROGRAM_WORD  0
+static inline HAL_StatusTypeDef HAL_FLASH_Unlock(void) { return HAL_OK; }
+static inline HAL_StatusTypeDef HAL_FLASH_Lock(void)   { return HAL_OK; }
+static inline HAL_StatusTypeDef HAL_FLASHEx_Erase(FLASH_EraseInitTypeDef *e, uint32_t *err) { (void)e; *err = 0xFFFFFFFFUL; return HAL_OK; }
+static inline HAL_StatusTypeDef HAL_FLASH_Program(uint32_t t, uint32_t addr, uint64_t data) { (void)t; (void)addr; (void)data; return HAL_OK; }
+static inline uint32_t HAL_GetTick(void) { return 0; }
+#endif
+
+/* ---- Block flash/jlink/balance headers (not needed) ---- */
+/* pid_flash.h is included via pid_schedule.h -- stub flash functions */
+
+/* Forward-declare stubs for pid_flash functions (used by pid_schedule.c) */
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+/* Minimal pid_sched_entry_t and pid_flash stubs before pulling in schedule */
+#define PID_FLASH_H  /* prevent pid_flash.h from being re-included */
+
+/* Replicate types from pid_flash.h */
+#define PID_SCHED_MAX_BANDS     6u
+#define PID_SCHED_FLASH_ADDR    0x0807FF40UL
+#define PID_SCHED_MAGIC         0x534C5402UL
+#define PID_FLASH_STORE_ADDR    0x0807FFC0UL
+#define PID_FLASH_MAGIC         0x534C5401UL
+#define PID_FLASH_SECTOR        7
+#define PID_FLASH_SECTOR_VOLTAGE 3
+
+typedef struct __attribute__((packed)) {
+    float speed_mps;
+    float kp;
+    float ki;
+    float kd;
+} pid_sched_entry_t;
+
+typedef struct __attribute__((packed)) {
+    uint32_t         magic;
+    uint8_t          num_bands;
+    uint8_t          flags;
+    uint8_t          _pad0[2];
+    pid_sched_entry_t bands[PID_SCHED_MAX_BANDS];
+    uint8_t          _pad1[24];
+} pid_sched_flash_t;
+
+typedef struct __attribute__((packed)) {
+    uint32_t magic;
+    float    kp;
+    float    ki;
+    float    kd;
+    uint8_t  _pad[48];
+} pid_flash_t;
+
+/* Stub flash storage (simulated in RAM) */
+static pid_sched_flash_t g_sched_flash;
+static pid_flash_t       g_pid_flash;
+static bool              g_sched_flash_valid = false;
+static bool              g_pid_flash_valid   = false;
+
+bool pid_flash_load(float *kp, float *ki, float *kd)
+{
+    if (!g_pid_flash_valid || g_pid_flash.magic != PID_FLASH_MAGIC) return false;
+    *kp = g_pid_flash.kp; *ki = g_pid_flash.ki; *kd = g_pid_flash.kd;
+    return true;
+}
+
+bool pid_flash_save(float kp, float ki, float kd)
+{
+    g_pid_flash.magic = PID_FLASH_MAGIC;
+    g_pid_flash.kp = kp; g_pid_flash.ki = ki; g_pid_flash.kd = kd;
+    g_pid_flash_valid = true;
+    return true;
+}
+
+bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n)
+{
+    if (!g_sched_flash_valid || g_sched_flash.magic != PID_SCHED_MAGIC) return false;
+    if (g_sched_flash.num_bands == 0 || g_sched_flash.num_bands > PID_SCHED_MAX_BANDS) return false;
+    memcpy(out_entries, g_sched_flash.bands, g_sched_flash.num_bands * sizeof(pid_sched_entry_t));
+    *out_n = g_sched_flash.num_bands;
+    return true;
+}
+
+bool pid_flash_save_all(float kp_s, float ki_s, float kd_s,
+                        const pid_sched_entry_t *entries, uint8_t num_bands)
+{
+    if (num_bands == 0 || num_bands > PID_SCHED_MAX_BANDS) return false;
+    g_sched_flash.magic = PID_SCHED_MAGIC;
+    g_sched_flash.num_bands = num_bands;
+    memcpy(g_sched_flash.bands, entries, num_bands * sizeof(pid_sched_entry_t));
+    g_sched_flash_valid = true;
+    g_pid_flash.magic = PID_FLASH_MAGIC;
+    g_pid_flash.kp = kp_s; g_pid_flash.ki = ki_s; g_pid_flash.kd = kd_s;
+    g_pid_flash_valid = true;
+    return true;
+}
+
+/* Stub mpu6000.h and balance.h so pid_schedule.h doesn't pull in hardware types */
+#define MPU6000_H
+typedef struct { float ax, ay, az, gx, gy, gz, pitch, pitch_rate; } IMUData;
+
+#define BALANCE_H
+typedef enum { BALANCE_DISARMED=0, BALANCE_ARMED, BALANCE_TILT_FAULT } balance_state_t;
+typedef struct {
+    balance_state_t state;
+    float pitch_deg, pitch_rate;
+    float integral, prev_error;
+    int16_t motor_cmd;
+    float kp, ki, kd, setpoint, max_tilt;
+    int16_t max_speed;
+} balance_t;
+
+/* Include the implementation directly */
+#include "../src/pid_schedule.c"
+
+/* ============================================================
+ * Test framework
+ * ============================================================ */
+static int g_pass = 0, g_fail = 0;
+
+#define ASSERT(cond, msg) do { \
+    if (cond) { g_pass++; } \
+    else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \
+} while (0)
+
+#define ASSERT_NEAR(a, b, eps, msg) ASSERT(fabsf((a)-(b)) < (eps), msg)
+
+static void reset_flash(void)
+{
+    g_sched_flash_valid = false;
+    g_pid_flash_valid   = false;
+    memset(&g_sched_flash, 0xFF, sizeof(g_sched_flash));
+    memset(&g_pid_flash,   0xFF, sizeof(g_pid_flash));
+}
+
+/* ============================================================
+ * Tests
+ * ============================================================ */
+
+static void test_init_loads_default_when_flash_empty(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    ASSERT(pid_schedule_get_num_bands() == 3u, "default 3 bands");
+    pid_sched_entry_t tbl[PID_SCHED_MAX_BANDS];
+    uint8_t n;
+    pid_schedule_get_table(tbl, &n);
+    ASSERT(n == 3u, "get_table returns 3");
+    ASSERT_NEAR(tbl[0].speed_mps, 0.00f, 1e-5f, "band0 speed=0.00");
+    ASSERT_NEAR(tbl[0].kp, 40.0f, 1e-4f, "band0 kp=40");
+    ASSERT_NEAR(tbl[2].speed_mps, 0.80f, 1e-5f, "band2 speed=0.80");
+    ASSERT_NEAR(tbl[2].kp, 28.0f, 1e-4f, "band2 kp=28");
+}
+
+static void test_init_loads_from_flash_when_valid(void)
+{
+    reset_flash();
+    pid_sched_entry_t entries[2] = {
+        { .speed_mps = 0.0f, .kp = 10.0f, .ki = 0.5f, .kd = 0.2f },
+        { .speed_mps = 1.0f, .kp = 20.0f, .ki = 0.8f, .kd = 0.4f },
+    };
+    pid_flash_save_all(1.0f, 0.1f, 0.1f, entries, 2u);
+    pid_schedule_init();
+    ASSERT(pid_schedule_get_num_bands() == 2u, "init loads 2 bands from flash");
+    pid_sched_entry_t tbl[PID_SCHED_MAX_BANDS];
+    uint8_t n;
+    pid_schedule_get_table(tbl, &n);
+    ASSERT_NEAR(tbl[1].kp, 20.0f, 1e-4f, "flash band1 kp=20");
+}
+
+static void test_get_gains_below_first_band(void)
+{
+    reset_flash();
+    pid_schedule_init();  /* default table: 0.0, 0.3, 0.8 m/s */
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.0f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 40.0f, 1e-4f, "speed=0 -> kp=40 (clamp low)");
+    /* abs(-0.1)=0.1 m/s: between band0(0.0) and band1(0.3), t=1/3 -> kp=40+(35-40)/3 */
+    pid_schedule_get_gains(-0.1f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 40.0f + (35.0f - 40.0f) * (0.1f / 0.3f), 0.01f,
+                "speed=-0.1 interpolates via abs(speed)");
+}
+
+static void test_get_gains_above_last_band(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(2.0f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 28.0f, 1e-4f, "speed=2.0 -> kp=28 (clamp high)");
+}
+
+static void test_get_gains_at_band_boundary(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.30f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 35.0f, 1e-4f, "speed=0.30 exactly -> kp=35");
+    pid_schedule_get_gains(0.80f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 28.0f, 1e-4f, "speed=0.80 exactly -> kp=28");
+}
+
+static void test_interpolation_midpoint(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    /* Between band0 (0.0,kp=40) and band1 (0.3,kp=35): at t=0.5 -> kp=37.5 */
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.15f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 37.5f, 0.01f, "interp midpoint kp=37.5");
+    /* Between band1 (0.3,kp=35) and band2 (0.8,kp=28): at t=0.2 -> 35+(28-35)*0.2=33.6 */
+    pid_schedule_get_gains(0.40f, &kp, &ki, &kd);
+    float expected = 35.0f + (28.0f - 35.0f) * ((0.40f - 0.30f) / (0.80f - 0.30f));
+    ASSERT_NEAR(kp, expected, 0.01f, "interp band1->2 kp");
+}
+
+static void test_interpolation_ki_kd(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.15f, &kp, &ki, &kd);
+    /* ki: band0=1.5, band1=1.0, t=0.5 -> 1.25 */
+    ASSERT_NEAR(ki, 1.25f, 0.01f, "interp midpoint ki=1.25");
+    /* kd: band0=1.2, band1=1.0, t=0.5 -> 1.1 */
+    ASSERT_NEAR(kd, 1.1f,  0.01f, "interp midpoint kd=1.1");
+}
+
+static void test_set_table_and_sort(void)
+{
+    pid_sched_entry_t tbl[3] = {
+        { .speed_mps = 0.8f, .kp = 5.0f, .ki = 0.1f, .kd = 0.1f },
+        { .speed_mps = 0.0f, .kp = 9.0f, .ki = 0.3f, .kd = 0.3f },
+        { .speed_mps = 0.4f, .kp = 7.0f, .ki = 0.2f, .kd = 0.2f },
+    };
+    pid_schedule_set_table(tbl, 3u);
+    ASSERT(pid_schedule_get_num_bands() == 3u, "set_table 3 bands");
+    pid_sched_entry_t out[PID_SCHED_MAX_BANDS];
+    uint8_t n;
+    pid_schedule_get_table(out, &n);
+    /* After sort: 0.0, 0.4, 0.8 */
+    ASSERT_NEAR(out[0].speed_mps, 0.0f, 1e-5f, "sorted[0]=0.0");
+    ASSERT_NEAR(out[1].speed_mps, 0.4f, 1e-5f, "sorted[1]=0.4");
+    ASSERT_NEAR(out[2].speed_mps, 0.8f, 1e-5f, "sorted[2]=0.8");
+}
+
+static void test_set_table_clamps_n(void)
+{
+    pid_sched_entry_t big[8];
+    memset(big, 0, sizeof(big));
+    for (int i = 0; i < 8; i++) big[i].speed_mps = (float)i * 0.1f;
+    pid_schedule_set_table(big, 8u);
+    ASSERT(pid_schedule_get_num_bands() == PID_SCHED_MAX_BANDS, "clamp to MAX_BANDS");
+}
+
+static void test_set_table_min_1(void)
+{
+    pid_sched_entry_t one = { .speed_mps = 0.5f, .kp = 30.0f, .ki = 1.0f, .kd = 0.8f };
+    pid_schedule_set_table(&one, 0u);  /* n=0 clamped to 1 */
+    ASSERT(pid_schedule_get_num_bands() == 1u, "min 1 band");
+}
+
+static void test_active_band_idx_clamp_low(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.0f, &kp, &ki, &kd);
+    ASSERT(pid_schedule_active_band_idx() == 0u, "active=0 when clamped low");
+}
+
+static void test_active_band_idx_interpolating(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.5f, &kp, &ki, &kd);  /* between band1 and band2 */
+    ASSERT(pid_schedule_active_band_idx() == 1u, "active=1 between band1-2");
+}
+
+static void test_active_band_idx_clamp_high(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(5.0f, &kp, &ki, &kd);
+    ASSERT(pid_schedule_active_band_idx() == 2u, "active=2 when clamped high");
+}
+
+static void test_apply_writes_gains(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    balance_t b;
+    memset(&b, 0, sizeof(b));
+    pid_schedule_apply(&b, 0.0f);
+    ASSERT_NEAR(b.kp, 40.0f, 1e-4f, "apply: kp written");
+    ASSERT_NEAR(b.ki, 1.5f,  1e-4f, "apply: ki written");
+    ASSERT_NEAR(b.kd, 1.2f,  1e-4f, "apply: kd written");
+}
+
+static void test_apply_resets_integral_on_band_change(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    balance_t b;
+    memset(&b, 0, sizeof(b));
+    b.integral = 99.0f;
+
+    /* First call: sets s_prev_band from sentinel -> band 0 (integral reset) */
+    pid_schedule_apply(&b, 0.0f);
+    ASSERT_NEAR(b.integral, 0.0f, 1e-6f, "apply: integral reset on first call");
+
+    b.integral = 77.0f;
+    pid_schedule_apply(&b, 0.0f);  /* same band -- no reset */
+    ASSERT_NEAR(b.integral, 77.0f, 1e-6f, "apply: integral preserved same band");
+
+    b.integral = 55.0f;
+    pid_schedule_apply(&b, 0.5f);  /* band changes 0->1 -- reset */
+    ASSERT_NEAR(b.integral, 0.0f, 1e-6f, "apply: integral reset on band change");
+}
+
+static void test_flash_save_and_reload(void)
+{
+    reset_flash();
+    pid_sched_entry_t tbl[2] = {
+        { .speed_mps = 0.0f, .kp = 15.0f, .ki = 0.6f, .kd = 0.3f },
+        { .speed_mps = 0.5f, .kp = 10.0f, .ki = 0.4f, .kd = 0.2f },
+    };
+    pid_schedule_set_table(tbl, 2u);
+    bool ok = pid_schedule_flash_save(25.0f, 1.1f, 0.9f);
+    ASSERT(ok, "flash_save returns true");
+    ASSERT(g_sched_flash_valid, "flash_save wrote sched record");
+    ASSERT(g_pid_flash_valid,   "flash_save wrote pid record");
+    ASSERT_NEAR(g_pid_flash.kp, 25.0f, 1e-4f, "pid kp saved");
+
+    /* Now reload */
+    pid_schedule_init();
+    ASSERT(pid_schedule_get_num_bands() == 2u, "reload 2 bands");
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.0f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 15.0f, 1e-4f, "reload kp at speed=0");
+}
+
+static void test_get_default_table(void)
+{
+    pid_sched_entry_t def[PID_SCHED_MAX_BANDS];
+    uint8_t n;
+    pid_schedule_get_default_table(def, &n);
+    ASSERT(n == 3u, "default table has 3 entries");
+    ASSERT_NEAR(def[0].kp, 40.0f, 1e-4f, "default[0] kp=40");
+    ASSERT_NEAR(def[1].kp, 35.0f, 1e-4f, "default[1] kp=35");
+    ASSERT_NEAR(def[2].kp, 28.0f, 1e-4f, "default[2] kp=28");
+}
+
+static void test_init_discards_invalid_flash(void)
+{
+    reset_flash();
+    /* Write a valid record but with out-of-range gain */
+    pid_sched_entry_t bad[1] = {{ .speed_mps=0.0f, .kp=999.0f, .ki=0.1f, .kd=0.1f }};
+    pid_flash_save_all(1.0f, 0.1f, 0.1f, bad, 1u);
+    pid_schedule_init();
+    /* Should fall back to default */
+    ASSERT(pid_schedule_get_num_bands() == 3u, "invalid flash -> default 3 bands");
+}
+
+static void test_single_band_clamps_both_ends(void)
+{
+    pid_sched_entry_t one = { .speed_mps = 0.5f, .kp = 50.0f, .ki = 2.0f, .kd = 1.5f };
+    pid_schedule_set_table(&one, 1u);
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.0f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 50.0f, 1e-4f, "single band: clamp low -> kp=50");
+    pid_schedule_get_gains(9.9f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 50.0f, 1e-4f, "single band: clamp high -> kp=50");
+}
+
+static void test_negative_speed_symmetric(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp_fwd, ki_fwd, kd_fwd;
+    float kp_rev, ki_rev, kd_rev;
+    pid_schedule_get_gains( 0.5f, &kp_fwd, &ki_fwd, &kd_fwd);
+    pid_schedule_get_gains(-0.5f, &kp_rev, &ki_rev, &kd_rev);
+    ASSERT_NEAR(kp_fwd, kp_rev, 1e-5f, "symmetric: kp same for +/-speed");
+    ASSERT_NEAR(ki_fwd, ki_rev, 1e-5f, "symmetric: ki same for +/-speed");
+    ASSERT_NEAR(kd_fwd, kd_rev, 1e-5f, "symmetric: kd same for +/-speed");
+}
+
+int main(void)
+{
+    printf("=== test_pid_schedule ===\n");
+
+    test_init_loads_default_when_flash_empty();
+    test_init_loads_from_flash_when_valid();
+    test_get_gains_below_first_band();
+    test_get_gains_above_last_band();
+    test_get_gains_at_band_boundary();
+    test_interpolation_midpoint();
+    test_interpolation_ki_kd();
+    test_set_table_and_sort();
+    test_set_table_clamps_n();
+    test_set_table_min_1();
+    test_active_band_idx_clamp_low();
+    test_active_band_idx_interpolating();
+    test_active_band_idx_clamp_high();
+    test_apply_writes_gains();
+    test_apply_resets_integral_on_band_change();
+    test_flash_save_and_reload();
+    test_get_default_table();
+    test_init_discards_invalid_flash();
+    test_single_band_clamps_both_ends();
+    test_negative_speed_symmetric();
+
+    printf("PASSED: %d  FAILED: %d\n", g_pass, g_fail);
+    return (g_fail == 0) ? 0 : 1;
+}