include/jlink.h

@@ -50,6 +50,8 @@
  *   0x87  FAULT_LOG     - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
  *   0x88  SLOPE         - jlink_tlm_slope_t (4 bytes), sent at SLOPE_TLM_HZ (Issue #600)
  *   0x89  CAN_STATS     - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597)
+ *   0x8A  STEERING      - jlink_tlm_steering_t (8 bytes), sent at STEER_TLM_HZ (Issue #616)
+ *   0x8B  LVC           - jlink_tlm_lvc_t (4 bytes), sent at LVC_TLM_HZ (Issue #613)
  *
  * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
  * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
@@ -93,6 +95,8 @@
 #define JLINK_TLM_FAULT_LOG     0x87u  /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
 #define JLINK_TLM_SLOPE         0x88u  /* jlink_tlm_slope_t (4 bytes, Issue #600) */
 #define JLINK_TLM_CAN_STATS     0x89u  /* jlink_tlm_can_stats_t (16 bytes, Issue #597) */
+#define JLINK_TLM_STEERING      0x8Au  /* jlink_tlm_steering_t (8 bytes, Issue #616) */
+#define JLINK_TLM_LVC           0x8Bu  /* jlink_tlm_lvc_t (4 bytes, Issue #613) */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
 typedef struct __attribute__((packed)) {
@@ -204,6 +208,24 @@ typedef struct __attribute__((packed)) {
     int16_t  vel1_rpm;     /* node 1 current velocity (RPM)               */
 } jlink_tlm_can_stats_t;  /* 16 bytes */
 
+/* ---- Telemetry STEERING payload (8 bytes, packed) Issue #616 ---- */
+/* Published at STEER_TLM_HZ (10 Hz); reports yaw-rate PID state. */
+typedef struct __attribute__((packed)) {
+    int16_t  target_x10;  /* target yaw rate, deg/s × 10 (0.1 deg/s resolution) */
+    int16_t  actual_x10;  /* measured yaw rate, deg/s × 10                       */
+    int16_t  output;      /* differential motor output (-STEER_OUTPUT_MAX..+MAX)  */
+    uint8_t  enabled;     /* 1 = PID active                                       */
+    uint8_t  _pad;        /* reserved                                             */
+} jlink_tlm_steering_t;  /* 8 bytes */
+
+/* ---- Telemetry LVC payload (4 bytes, packed) Issue #613 ---- */
+/* Sent at LVC_TLM_HZ (1 Hz); reports battery voltage and LVC protection state. */
+typedef struct __attribute__((packed)) {
+    uint16_t voltage_mv;        /* battery voltage (mV)                                    */
+    uint8_t  percent;           /* 0-100: fuel gauge within CUTOFF..WARNING; 255=unknown   */
+    uint8_t  protection_state;  /* LvcState: 0=NORMAL,1=WARNING,2=CRITICAL,3=CUTOFF        */
+} jlink_tlm_lvc_t;  /* 4 bytes */
+
 /* ---- Volatile state (read from main loop) ---- */
 typedef struct {
     /* Drive command - updated on JLINK_CMD_DRIVE */
@@ -322,4 +344,17 @@ void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
  */
 void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm);
 
+/*
+ * jlink_send_steering_tlm(tlm) - transmit JLINK_TLM_STEERING (0x8A) frame
+ * (14 bytes total) to Jetson.  Issue #616.
+ * Rate-limiting is handled by steering_pid_send_tlm(); call from there only.
+ */
+void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm);
+
+/*
+ * jlink_send_lvc_tlm(tlm) - transmit JLINK_TLM_LVC (0x8B) frame
+ * (10 bytes total) at LVC_TLM_HZ (1 Hz).  Issue #613.
+ */
+void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm);
+
 #endif /* JLINK_H */

include/mpu6000.h

@@ -9,6 +9,7 @@ typedef struct {
     float pitch_rate;   // degrees/sec (raw gyro pitch axis)
     float roll;         // degrees, filtered (complementary filter)
     float yaw;          // degrees, gyro-integrated (drifts — no magnetometer)
+    float yaw_rate;     // degrees/sec (raw gyro Z / board_gz, Issue #616)
     float accel_x;      // g
     float accel_z;      // g
 } IMUData;

include/steering_pid.h

@@ -0,0 +1,134 @@
+#ifndef STEERING_PID_H
+#define STEERING_PID_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * steering_pid — closed-loop yaw-rate controller for differential drive
+ * (Issue #616).
+ *
+ * OVERVIEW:
+ *   Converts a desired yaw rate (from Jetson Twist.angular.z) into a
+ *   differential wheel speed offset.  The balance PID remains the primary
+ *   controller; the steering PID generates a small differential term that
+ *   is added to the balance command inside motor_driver:
+ *
+ *     left_speed  = balance_cmd - steer_out
+ *     right_speed = balance_cmd + steer_out
+ *
+ *   This is the standard differential-drive mixing already performed by
+ *   the ESC backend (hoverboard/VESC).
+ *
+ * INPUT SIGNALS:
+ *   target_omega_dps : desired yaw rate in deg/s (+ = clockwise from above)
+ *                      Derived from JLINK_CMD_DRIVE steer field:
+ *                        target_omega_dps = steer * STEER_OMEGA_SCALE
+ *                      (steer is int16 -1000..+1000 from Jetson)
+ *   actual_omega_dps : measured yaw rate from IMU gyro Z (deg/s)
+ *                      = IMUData.yaw_rate
+ *
+ * PID ALGORITHM:
+ *   error     = target_omega - actual_omega
+ *   integral  = clamp(integral + error*dt, ±STEER_INTEGRAL_MAX)
+ *   raw_out   = Kp*error + Ki*integral + Kd*(error - prev_error)/dt
+ *   raw_out   = clamp(raw_out, ±STEER_OUTPUT_MAX)
+ *   output    = rate_limit(raw_out, STEER_RAMP_RATE_PER_MS * dt_ms)
+ *
+ * ANTI-WINDUP:
+ *   Integral is clamped to ±STEER_INTEGRAL_MAX counts before the Ki
+ *   multiply, bounding the integrator contribution independently of Kp/Kd.
+ *
+ * RATE LIMITER:
+ *   Output changes at most STEER_RAMP_RATE_PER_MS counts per millisecond.
+ *   Prevents a sudden step in steering demand from disturbing the balance
+ *   PID (which has no knowledge of the steering channel).
+ *
+ * OMEGA SCALING:
+ *   STEER_OMEGA_SCALE = 0.1 deg/s per steer unit.
+ *   Range: steer -1000..+1000 → omega -100..+100 deg/s.
+ *   100 deg/s ≈ 1.75 rad/s — covers aggressive turns without exceeding
+ *   the hoverboard ESC differential authority (STEER_OUTPUT_MAX = 400).
+ *
+ * DISABLING:
+ *   steering_pid_set_enabled(s, false) zeroes target_omega and integral,
+ *   then freezes output at 0.  Use when Jetson is not active or in
+ *   RC_MANUAL mode to avoid fighting the RC steer channel.
+ *
+ * TELEMETRY:
+ *   JLINK_TLM_STEERING (0x8A) published at STEER_TLM_HZ (10 Hz):
+ *     jlink_tlm_steering_t { int16 target_x10, int16 actual_x10,
+ *                            int16 output, uint8 enabled, uint8 _pad }
+ *   8 bytes total.
+ */
+
+/* ---- Configuration ---- */
+#define STEER_KP               2.0f   /* proportional gain (counts / (deg/s))  */
+#define STEER_KI               0.5f   /* integral gain (counts / (deg))         */
+#define STEER_KD               0.05f  /* derivative gain (counts / (deg/s²))   */
+#define STEER_INTEGRAL_MAX    200.0f  /* integrator clamp (motor counts)        */
+#define STEER_OUTPUT_MAX       400    /* peak differential output (counts)      */
+#define STEER_RAMP_RATE_PER_MS  10    /* max output change per ms (counts/ms)   */
+#define STEER_OMEGA_SCALE      0.1f   /* steer units → deg/s (0.1 deg/s/unit)  */
+#define STEER_TLM_HZ           10u    /* JLINK_TLM_STEERING publish rate (Hz)  */
+
+/* ---- State ---- */
+typedef struct {
+    float    target_omega_dps;  /* setpoint: desired yaw rate (deg/s)       */
+    float    actual_omega_dps;  /* feedback: measured yaw rate (deg/s)      */
+    float    integral;          /* PID integrator (motor counts·s)          */
+    float    prev_error;        /* error at last update (deg/s)             */
+    int16_t  output;            /* rate-limited differential output          */
+    bool     enabled;           /* false = output held at 0                 */
+    uint32_t last_tlm_ms;       /* rate-limit for TLM                       */
+} steering_pid_t;
+
+/* ---- API ---- */
+
+/*
+ * steering_pid_init(s) — zero state, enable controller.
+ * Call once during system init.
+ */
+void steering_pid_init(steering_pid_t *s);
+
+/*
+ * steering_pid_reset(s) — zero integrator, setpoint and output.
+ * Preserves enabled flag.  Call on disarm.
+ */
+void steering_pid_reset(steering_pid_t *s);
+
+/*
+ * steering_pid_set_target(s, omega_dps) — update setpoint.
+ *   omega_dps : desired yaw rate in deg/s.
+ *   Converts from JLINK_CMD_DRIVE steer field: omega = steer * STEER_OMEGA_SCALE.
+ *   No-op if disabled (output remains 0).
+ */
+void steering_pid_set_target(steering_pid_t *s, float omega_dps);
+
+/*
+ * steering_pid_update(s, actual_omega_dps, dt) — advance PID one step.
+ *   actual_omega_dps : IMU gyro Z rate (deg/s) — use IMUData.yaw_rate.
+ *   dt               : loop interval (seconds).
+ * Returns differential output (-STEER_OUTPUT_MAX..+STEER_OUTPUT_MAX).
+ * Returns 0 if disabled or dt <= 0.
+ */
+int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt);
+
+/*
+ * steering_pid_get_output(s) — last computed differential output.
+ */
+int16_t steering_pid_get_output(const steering_pid_t *s);
+
+/*
+ * steering_pid_set_enabled(s, en) — enable or disable the controller.
+ * Disabling resets integrator and zeroes output.
+ */
+void steering_pid_set_enabled(steering_pid_t *s, bool en);
+
+/*
+ * steering_pid_send_tlm(s, now_ms) — transmit JLINK_TLM_STEERING (0x8A)
+ * frame to Jetson.  Rate-limited to STEER_TLM_HZ; safe to call every tick.
+ */
+void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms);
+
+#endif /* STEERING_PID_H */

src/jlink.c

@@ -618,3 +618,27 @@ void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm)
 
     jlink_tx_locked(frame, sizeof(frame));
 }
+
+/* ---- jlink_send_lvc_tlm() -- Issue #613 ---- */
+void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm)
+{
+    /*
+     * Frame: [STX][LEN][0x8B][4 bytes LVC][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 + 4 = 5; total = 10 bytes
+     */
+    static uint8_t frame[10];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_lvc_t);  /* 4 */
+    const uint8_t  len  = 1u + plen;                          /* 5 */
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_LVC;
+    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));
+}

src/mpu6000.c

@@ -151,6 +151,7 @@ void mpu6000_read(IMUData *data) {
     data->pitch_rate = gyro_pitch_rate;
     data->roll       = s_roll;
     data->yaw        = s_yaw;
+    data->yaw_rate   = gyro_yaw_rate;  /* board_gz: raw bias-corrected gyro Z (Issue #616) */
     data->accel_x    = ax;
     data->accel_z    = az;
 }

src/steering_pid.c

@@ -0,0 +1,140 @@
+/*
+ * steering_pid.c — closed-loop yaw-rate PID for differential drive (Issue #616).
+ *
+ * Converts Jetson Twist.angular.z (encoded as steer * STEER_OMEGA_SCALE deg/s)
+ * into a differential wheel speed offset using IMU gyro Z as feedback.
+ *
+ * Anti-windup:   integral clamped to ±STEER_INTEGRAL_MAX before Ki multiply.
+ * Rate limiter:  output changes at most STEER_RAMP_RATE_PER_MS per ms so that
+ *                a step in steering demand cannot disturb the balance PID.
+ */
+
+#include "steering_pid.h"
+#include "jlink.h"
+
+/* ---- steering_pid_init() ---- */
+void steering_pid_init(steering_pid_t *s)
+{
+    s->target_omega_dps = 0.0f;
+    s->actual_omega_dps = 0.0f;
+    s->integral         = 0.0f;
+    s->prev_error       = 0.0f;
+    s->output           = 0;
+    s->enabled          = true;
+    /* Initialize so first send_tlm call fires immediately */
+    s->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (STEER_TLM_HZ > 0u ? STEER_TLM_HZ : 1u)));
+}
+
+/* ---- steering_pid_reset() ---- */
+void steering_pid_reset(steering_pid_t *s)
+{
+    s->target_omega_dps = 0.0f;
+    s->integral         = 0.0f;
+    s->prev_error       = 0.0f;
+    s->output           = 0;
+}
+
+/* ---- steering_pid_set_target() ---- */
+void steering_pid_set_target(steering_pid_t *s, float omega_dps)
+{
+    if (!s->enabled) return;
+    s->target_omega_dps = omega_dps;
+}
+
+/* ---- steering_pid_update() ---- */
+int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt)
+{
+    if (!s->enabled || dt <= 0.0f) {
+        s->output = 0;
+        return 0;
+    }
+
+    s->actual_omega_dps = actual_omega_dps;
+
+    /* PID error */
+    float error = s->target_omega_dps - actual_omega_dps;
+
+    /* Proportional */
+    float p_term = STEER_KP * error;
+
+    /* Integral with anti-windup clamp */
+    s->integral += error * dt;
+    if (s->integral >  STEER_INTEGRAL_MAX) s->integral =  STEER_INTEGRAL_MAX;
+    if (s->integral < -STEER_INTEGRAL_MAX) s->integral = -STEER_INTEGRAL_MAX;
+    float i_term = STEER_KI * s->integral;
+
+    /* Derivative on error (avoids setpoint kick for smooth yaw changes) */
+    float d_term = 0.0f;
+    if (dt > 0.0f) {
+        d_term = STEER_KD * (error - s->prev_error) / dt;
+    }
+    s->prev_error = error;
+
+    /* Sum and clamp raw output */
+    float raw = p_term + i_term + d_term;
+    if (raw >  (float)STEER_OUTPUT_MAX) raw =  (float)STEER_OUTPUT_MAX;
+    if (raw < -(float)STEER_OUTPUT_MAX) raw = -(float)STEER_OUTPUT_MAX;
+
+    /* Rate limiter: bound change per step by STEER_RAMP_RATE_PER_MS * dt */
+    float max_step = (float)STEER_RAMP_RATE_PER_MS * (dt * 1000.0f);
+    float delta = raw - (float)s->output;
+    if (delta >  max_step) delta =  max_step;
+    if (delta < -max_step) delta = -max_step;
+
+    float limited = (float)s->output + delta;
+    /* Final clamp after rate limit */
+    if (limited >  (float)STEER_OUTPUT_MAX) limited =  (float)STEER_OUTPUT_MAX;
+    if (limited < -(float)STEER_OUTPUT_MAX) limited = -(float)STEER_OUTPUT_MAX;
+
+    s->output = (int16_t)limited;
+    return s->output;
+}
+
+/* ---- steering_pid_get_output() ---- */
+int16_t steering_pid_get_output(const steering_pid_t *s)
+{
+    return s->output;
+}
+
+/* ---- steering_pid_set_enabled() ---- */
+void steering_pid_set_enabled(steering_pid_t *s, bool en)
+{
+    if (!en && s->enabled) {
+        /* Disabling: zero out state */
+        s->target_omega_dps = 0.0f;
+        s->integral         = 0.0f;
+        s->prev_error       = 0.0f;
+        s->output           = 0;
+    }
+    s->enabled = en;
+}
+
+/* ---- steering_pid_send_tlm() ---- */
+void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms)
+{
+    if (STEER_TLM_HZ == 0u) return;
+
+    uint32_t interval_ms = 1000u / STEER_TLM_HZ;
+    if ((now_ms - s->last_tlm_ms) < interval_ms) return;
+    /* Cast away const for timestamp update — only mutable field */
+    ((steering_pid_t *)s)->last_tlm_ms = now_ms;
+
+    jlink_tlm_steering_t tlm;
+
+    /* Scale to ×10 for 0.1 deg/s resolution, clamped to int16 range */
+    float t = s->target_omega_dps * 10.0f;
+    if (t >  32767.0f) t =  32767.0f;
+    if (t < -32768.0f) t = -32768.0f;
+    tlm.target_x10 = (int16_t)t;
+
+    float a = s->actual_omega_dps * 10.0f;
+    if (a >  32767.0f) a =  32767.0f;
+    if (a < -32768.0f) a = -32768.0f;
+    tlm.actual_x10 = (int16_t)a;
+
+    tlm.output  = s->output;
+    tlm.enabled = s->enabled ? 1u : 0u;
+    tlm._pad    = 0u;
+
+    jlink_send_steering_tlm(&tlm);
+}

test/test_steering_pid.c

@@ -0,0 +1,618 @@
+/*
+ * test_steering_pid.c — unit tests for steering_pid (Issue #616).
+ *
+ * Host build (no HAL):
+ *   gcc -I include -I src -DTEST_HOST -lm -o /tmp/test_steering test/test_steering_pid.c
+ *   /tmp/test_steering
+ *
+ * Stubs out jlink.h to avoid the full HAL dependency chain.
+ */
+
+/* ---- Minimal stubs ---- */
+#define JLINK_H  /* prevent real jlink.h from loading */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* Minimal jlink_tlm_steering_t and send stub */
+typedef struct __attribute__((packed)) {
+    int16_t  target_x10;
+    int16_t  actual_x10;
+    int16_t  output;
+    uint8_t  enabled;
+    uint8_t  _pad;
+} jlink_tlm_steering_t;
+
+static jlink_tlm_steering_t g_last_tlm;
+static int g_tlm_count = 0;
+
+void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm)
+{
+    g_last_tlm = *tlm;
+    g_tlm_count++;
+}
+
+/* ---- Include the module under test ---- */
+#include "steering_pid.h"
+#include "steering_pid.c"   /* single-TU build; steering_pid.c found via -I src */
+
+/* ---- Test framework ---- */
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+
+static int s_pass = 0;
+static int s_fail = 0;
+
+#define EXPECT(cond, msg) do { \
+    if (cond) { s_pass++; } \
+    else { s_fail++; printf("FAIL [%s:%d]: %s\n", __func__, __LINE__, msg); } \
+} while(0)
+
+#define EXPECT_EQ(a, b, msg)  EXPECT((a) == (b), msg)
+#define EXPECT_NEAR(a, b, eps, msg) EXPECT(fabsf((a) - (b)) <= (eps), msg)
+#define EXPECT_TRUE(x, msg)   EXPECT((x), msg)
+#define EXPECT_FALSE(x, msg)  EXPECT(!(x), msg)
+
+/* ---- Tests ---- */
+
+static void test_init(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "init: target_omega 0");
+    EXPECT_NEAR(s.actual_omega_dps, 0.0f, 1e-6f, "init: actual_omega 0");
+    EXPECT_NEAR(s.integral,         0.0f, 1e-6f, "init: integral 0");
+    EXPECT_NEAR(s.prev_error,       0.0f, 1e-6f, "init: prev_error 0");
+    EXPECT_EQ  (s.output,           0,           "init: output 0");
+    EXPECT_TRUE(s.enabled,                        "init: enabled true");
+}
+
+static void test_reset(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = 30.0f;
+    s.integral  = 100.0f;
+    s.prev_error = 5.0f;
+    s.output     = 200;
+
+    steering_pid_reset(&s);
+
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "reset: target 0");
+    EXPECT_NEAR(s.integral,         0.0f, 1e-6f, "reset: integral 0");
+    EXPECT_NEAR(s.prev_error,       0.0f, 1e-6f, "reset: prev_error 0");
+    EXPECT_EQ  (s.output,           0,           "reset: output 0");
+    EXPECT_TRUE(s.enabled,                        "reset: preserves enabled");
+}
+
+static void test_disabled_returns_zero(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, false);
+    steering_pid_set_target(&s, 50.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
+
+    EXPECT_EQ(out, 0, "disabled: update returns 0");
+    EXPECT_EQ(steering_pid_get_output(&s), 0, "disabled: get_output 0");
+    /* Target should not have been set when disabled */
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "disabled: target not set");
+}
+
+static void test_zero_dt_returns_zero(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 30.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, 0.0f);
+    EXPECT_EQ(out, 0, "zero_dt: returns 0");
+}
+
+static void test_negative_dt_returns_zero(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 30.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, -0.01f);
+    EXPECT_EQ(out, 0, "neg_dt: returns 0");
+}
+
+static void test_proportional_only(void)
+{
+    /*
+     * With Ki=0 and Kd=0, output = Kp * error.
+     * Set STEER_KP=2.0, error=50 deg/s → raw = 100 counts.
+     * Rate limiter allows max_step = STEER_RAMP_RATE_PER_MS * dt_ms = 10 * 1 = 10 per step.
+     * After 1 step (dt=0.001s): output capped at 10 by rate limiter.
+     */
+    steering_pid_t s;
+    memset(&s, 0, sizeof(s));
+    steering_pid_init(&s);
+    s.integral   = 0.0f;
+    s.prev_error = 0.0f;
+    s.output     = 0;
+
+    steering_pid_set_target(&s, 50.0f);
+    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
+
+    /* Rate limiter: max_step = 10*0.001*1000 = 10 → output = 10 */
+    EXPECT_EQ(out, 10, "prop_only: rate-limited step");
+}
+
+static void test_converges_to_setpoint(void)
+{
+    /*
+     * With no disturbance, run many steps until output reaches the setpoint.
+     * With target=20 deg/s, Kp=2, after many steps the P term = 2*20 = 40,
+     * plus I accumulation. The rate limiter will eventually allow the output
+     * to grow. Run for 1s at 1kHz (1000 steps, dt=0.001s each).
+     * Expected: eventually output > 0 (PID working), integral accumulates.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 20.0f);
+
+    int16_t out = 0;
+    for (int i = 0; i < 1000; i++) {
+        out = steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    EXPECT_TRUE(out > 0, "convergence: output positive after 1s");
+    EXPECT_TRUE(s.integral > 0.0f, "convergence: integral positive");
+}
+
+static void test_output_clamped_to_max(void)
+{
+    /*
+     * Very large error should produce output clamped to STEER_OUTPUT_MAX.
+     * Run many ticks so rate limiter is satisfied.
+     * STEER_OUTPUT_MAX = 400; 400 ticks at 10 counts/ms should saturate.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);  /* 500 deg/s — way beyond robot capability */
+
+    int16_t out = 0;
+    for (int i = 0; i < 500; i++) {
+        out = steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    EXPECT_EQ(out, STEER_OUTPUT_MAX, "clamp: output saturates at STEER_OUTPUT_MAX");
+}
+
+static void test_output_clamped_negative(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, -500.0f);
+
+    int16_t out = 0;
+    for (int i = 0; i < 500; i++) {
+        out = steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    EXPECT_EQ(out, -STEER_OUTPUT_MAX, "clamp_neg: output saturates at -STEER_OUTPUT_MAX");
+}
+
+static void test_anti_windup(void)
+{
+    /*
+     * Run with a persistent error for a long time.
+     * Integral should be clamped to ±STEER_INTEGRAL_MAX.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 50.0f);
+
+    for (int i = 0; i < 10000; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);  /* actual stays 0, error = 50 */
+    }
+    EXPECT_NEAR(s.integral, STEER_INTEGRAL_MAX, 0.01f, "anti-windup: integral clamped to max");
+}
+
+static void test_anti_windup_negative(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, -50.0f);
+
+    for (int i = 0; i < 10000; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    EXPECT_NEAR(s.integral, -STEER_INTEGRAL_MAX, 0.01f, "anti-windup-neg: integral clamped");
+}
+
+static void test_rate_limiter_single_step(void)
+{
+    /*
+     * At dt=0.001s: max_step = STEER_RAMP_RATE_PER_MS * 1 = 10.
+     * Even if raw PID output = 400, first step can only reach 10.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
+    EXPECT_EQ(out, STEER_RAMP_RATE_PER_MS * 1, "rate_lim: first step bounded");
+}
+
+static void test_rate_limiter_larger_dt(void)
+{
+    /*
+     * At dt=0.01s (100Hz): max_step = 10 * 10 = 100.
+     * First step limited to 100.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, 0.01f);
+    EXPECT_EQ(out, STEER_RAMP_RATE_PER_MS * 10, "rate_lim_100hz: first step bounded");
+}
+
+static void test_rate_limiter_decreasing(void)
+{
+    /*
+     * After saturating to STEER_OUTPUT_MAX, set target to 0.
+     * Output should decrease at most STEER_RAMP_RATE_PER_MS per ms.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);
+    for (int i = 0; i < 500; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    /* Now output = STEER_OUTPUT_MAX = 400 */
+    int16_t before = s.output;
+    steering_pid_reset(&s);  /* reset integral and target */
+    /* output is also zeroed by reset — which is fine: the test is
+       really about rate limiting when driving toward 0 after a step */
+    (void)before;
+
+    /* Re-saturate, then drive to 0 with matching actual */
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);
+    for (int i = 0; i < 500; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    steering_pid_set_target(&s, 0.0f);
+    int16_t pre  = s.output;
+    int16_t post = steering_pid_update(&s, 0.0f, 0.001f);
+
+    /* Should decrease by no more than STEER_RAMP_RATE_PER_MS per tick */
+    int16_t delta = pre - post;
+    EXPECT_TRUE(delta <= STEER_RAMP_RATE_PER_MS, "rate_lim_dec: decrease bounded per tick");
+}
+
+static void test_zero_error_zero_output(void)
+{
+    /*
+     * When target == actual from the first tick, error = 0 and output stays 0.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 30.0f);
+    int16_t out = steering_pid_update(&s, 30.0f, 0.001f);
+    EXPECT_EQ(out, 0, "zero_error: no output when tracking exactly");
+}
+
+static void test_enable_disable_clears_state(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 50.0f);
+    for (int i = 0; i < 100; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+
+    /* Disable */
+    steering_pid_set_enabled(&s, false);
+
+    EXPECT_FALSE(s.enabled,                       "disable: enabled false");
+    EXPECT_NEAR (s.target_omega_dps, 0.0f, 1e-6f, "disable: target zeroed");
+    EXPECT_NEAR (s.integral,         0.0f, 1e-6f, "disable: integral zeroed");
+    EXPECT_EQ   (s.output,           0,           "disable: output zeroed");
+
+    /* Calls while disabled return 0 */
+    int16_t out = steering_pid_update(&s, 50.0f, 0.001f);
+    EXPECT_EQ(out, 0, "disable: update returns 0");
+}
+
+static void test_re_enable(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, false);
+    steering_pid_set_enabled(&s, true);
+
+    EXPECT_TRUE(s.enabled, "re-enable: enabled true");
+
+    /* After re-enable, new target and update should work */
+    steering_pid_set_target(&s, 20.0f);
+    EXPECT_NEAR(s.target_omega_dps, 20.0f, 1e-6f, "re-enable: set_target works");
+}
+
+static void test_set_target_ignored_when_disabled(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, false);
+    steering_pid_set_target(&s, 99.0f);
+
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "disabled_target: target not set");
+}
+
+static void test_actual_omega_updated(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_update(&s, 42.5f, 0.001f);
+    EXPECT_NEAR(s.actual_omega_dps, 42.5f, 1e-4f, "actual_omega: stored after update");
+}
+
+static void test_get_output_matches_update(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 20.0f);
+    int16_t from_update = steering_pid_update(&s, 0.0f, 0.001f);
+    EXPECT_EQ(from_update, steering_pid_get_output(&s), "get_output: matches update return");
+}
+
+static void test_omega_scale_convention(void)
+{
+    /*
+     * Verify that setting target via STEER_OMEGA_SCALE:
+     *   omega_dps = steer * STEER_OMEGA_SCALE
+     *   steer=100 → omega=10 deg/s
+     */
+    float omega = 100.0f * STEER_OMEGA_SCALE;
+    EXPECT_NEAR(omega, 10.0f, 1e-4f, "omega_scale: 100 units = 10 deg/s");
+
+    omega = -1000.0f * STEER_OMEGA_SCALE;
+    EXPECT_NEAR(omega, -100.0f, 1e-4f, "omega_scale: -1000 units = -100 deg/s");
+}
+
+static void test_tlm_first_call_fires(void)
+{
+    /*
+     * After init, last_tlm_ms is set so that the first send_tlm fires immediately
+     * at now_ms = 0.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_tlm_count, 1, "tlm_first: fires at ms=0 after init");
+}
+
+static void test_tlm_rate_limited(void)
+{
+    /*
+     * STEER_TLM_HZ = 10 → interval = 100ms.
+     * Two calls at ms=0 and ms=50 should only send once.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);    /* fires (first call) */
+    steering_pid_send_tlm(&s, 50);   /* blocked (< 100ms) */
+    EXPECT_EQ(g_tlm_count, 1, "tlm_rate: blocked within interval");
+
+    steering_pid_send_tlm(&s, 100);  /* fires (interval elapsed) */
+    EXPECT_EQ(g_tlm_count, 2, "tlm_rate: fires after interval");
+}
+
+static void test_tlm_payload_target(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = 25.5f;  /* → target_x10 = 255 */
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.target_x10, 255, "tlm_payload: target_x10 correct");
+}
+
+static void test_tlm_payload_actual(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 10.0f);
+    steering_pid_update(&s, 12.3f, 0.001f);
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);
+    /* actual = 12.3 deg/s → actual_x10 = 123 */
+    EXPECT_EQ(g_last_tlm.actual_x10, 123, "tlm_payload: actual_x10 correct");
+}
+
+static void test_tlm_payload_output(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.output = 150;
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.output, 150, "tlm_payload: output correct");
+}
+
+static void test_tlm_payload_enabled_flag(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, true);
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.enabled, 1u, "tlm_payload: enabled=1 when on");
+
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, false);
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.enabled, 0u, "tlm_payload: enabled=0 when off");
+}
+
+static void test_tlm_payload_pad_zero(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm._pad, 0u, "tlm_payload: _pad = 0");
+}
+
+static void test_tlm_target_x10_negative(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = -30.7f;  /* → target_x10 = -307 */
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.target_x10, -307, "tlm_payload: negative target_x10");
+}
+
+static void test_tlm_int16_clamp_positive(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = 4000.0f;  /* × 10 = 40000 > INT16_MAX */
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.target_x10, 32767, "tlm_clamp: target_x10 clamped to INT16_MAX");
+}
+
+static void test_tlm_int16_clamp_negative(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = -4000.0f;  /* × 10 = -40000 < INT16_MIN */
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.target_x10, (int16_t)-32768, "tlm_clamp: target_x10 clamped to INT16_MIN");
+}
+
+static void test_balance_not_disturbed_by_small_step(void)
+{
+    /*
+     * A balance bot is most sensitive to sudden changes in steer_cmd.
+     * Verify that a large target step at t=0 only increments output by
+     * STEER_RAMP_RATE_PER_MS per millisecond — not a step change.
+     * This confirms the rate limiter protects the balance PID.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 100.0f);   /* large yaw demand */
+
+    /* 10ms at 1kHz */
+    for (int i = 0; i < 10; i++) {
+        int16_t pre  = s.output;
+        int16_t post = steering_pid_update(&s, 0.0f, 0.001f);
+        int16_t step = post - pre;
+        EXPECT_TRUE(step <= STEER_RAMP_RATE_PER_MS, "balance_protect: step bounded each tick");
+        EXPECT_TRUE(step >= -STEER_RAMP_RATE_PER_MS, "balance_protect: neg step bounded each tick");
+    }
+}
+
+static void test_derivative_on_error_change(void)
+{
+    /*
+     * When error changes suddenly (e.g. actual_omega jumps), the derivative
+     * term should add a braking contribution.
+     * With STEER_KD=0.05 and a 10 deg/s²-equivalent error rate change:
+     *   d_term = 0.05 * (error1 - error0) / dt
+     * We just verify output changes when error changes, indicating D is active.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 20.0f);
+
+    /* Step 1: actual = 0, error = 20 */
+    int16_t out1 = steering_pid_update(&s, 0.0f, 0.001f);
+
+    /* Step 2: actual jumps to 20, error = 0. D will oppose. */
+    int16_t out2 = steering_pid_update(&s, 20.0f, 0.001f);
+
+    /* out2 should be less than out1 (derivative braking as error drops) */
+    EXPECT_TRUE(out2 <= out1, "derivative: braking when error shrinks");
+}
+
+static void test_reset_then_restart(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 50.0f);
+    for (int i = 0; i < 100; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+
+    steering_pid_reset(&s);
+    EXPECT_EQ  (s.output,            0,    "reset_restart: output 0");
+    EXPECT_NEAR(s.integral,  0.0f, 1e-6f,  "reset_restart: integral 0");
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "reset_restart: target 0");
+
+    /* After reset, new target should work cleanly */
+    steering_pid_set_target(&s, 10.0f);
+    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
+    EXPECT_TRUE(out > 0, "reset_restart: positive output after fresh start");
+}
+
+static void test_tlm_hz_constant(void)
+{
+    EXPECT_TRUE(STEER_TLM_HZ == 10u, "tlm_hz: STEER_TLM_HZ is 10");
+}
+
+static void test_output_max_constant(void)
+{
+    EXPECT_TRUE(STEER_OUTPUT_MAX == 400, "output_max: STEER_OUTPUT_MAX is 400");
+}
+
+/* ---- main ---- */
+int main(void)
+{
+    printf("=== test_steering_pid ===\n\n");
+
+    test_init();
+    test_reset();
+    test_disabled_returns_zero();
+    test_zero_dt_returns_zero();
+    test_negative_dt_returns_zero();
+    test_proportional_only();
+    test_converges_to_setpoint();
+    test_output_clamped_to_max();
+    test_output_clamped_negative();
+    test_anti_windup();
+    test_anti_windup_negative();
+    test_rate_limiter_single_step();
+    test_rate_limiter_larger_dt();
+    test_rate_limiter_decreasing();
+    test_zero_error_zero_output();
+    test_enable_disable_clears_state();
+    test_re_enable();
+    test_set_target_ignored_when_disabled();
+    test_actual_omega_updated();
+    test_get_output_matches_update();
+    test_omega_scale_convention();
+    test_tlm_first_call_fires();
+    test_tlm_rate_limited();
+    test_tlm_payload_target();
+    test_tlm_payload_actual();
+    test_tlm_payload_output();
+    test_tlm_payload_enabled_flag();
+    test_tlm_payload_pad_zero();
+    test_tlm_target_x10_negative();
+    test_tlm_int16_clamp_positive();
+    test_tlm_int16_clamp_negative();
+    test_balance_not_disturbed_by_small_step();
+    test_derivative_on_error_change();
+    test_reset_then_restart();
+    test_tlm_hz_constant();
+    test_output_max_constant();
+
+    printf("\nResults: %d passed, %d failed\n", s_pass, s_fail);
+    return s_fail ? 1 : 0;
+}