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/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_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;
+}