From 1e69337ffd15eed5abf6f4efe29dea21c7dee23f Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sun, 15 Mar 2026 10:11:05 -0400 Subject: [PATCH] feat: Steering PID for differential drive (Issue #616) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Closed-loop yaw-rate controller that converts Jetson Twist.angular.z to a differential wheel speed offset using IMU gyro Z as feedback. - include/steering_pid.h + src/steering_pid.c: PID with anti-windup (integral clamped to ±200 counts) and rate limiter (10 counts/ms max output change) to protect balance PID from sudden steering steps. JLINK_TLM_STEERING (0x8A) telemetry at 10 Hz. - include/mpu6000.h + src/mpu6000.c: expose yaw_rate (board_gz) in IMUData so callers have direct bias-corrected gyro Z feedback. - include/jlink.h + src/jlink.c: add JLINK_TLM_STEERING (0x8A), jlink_tlm_steering_t (8 bytes), jlink_send_steering_tlm(). - test/test_steering_pid.c: 78 unit tests (host build with gcc), all passing. Usage (main loop): steering_pid_set_target(&s, jlink_state.steer * STEER_OMEGA_SCALE); int16_t steer_out = steering_pid_update(&s, imu.yaw_rate, dt); motor_driver_update(&motor, balance_cmd, steer_out, now_ms); Co-Authored-By: Claude Sonnet 4.6 --- include/jlink.h | 35 +++ include/mpu6000.h | 1 + include/steering_pid.h | 134 +++++++++ src/jlink.c | 24 ++ src/mpu6000.c | 1 + src/steering_pid.c | 140 +++++++++ test/test_steering_pid.c | 618 +++++++++++++++++++++++++++++++++++++++ 7 files changed, 953 insertions(+) create mode 100644 include/steering_pid.h create mode 100644 src/steering_pid.c create mode 100644 test/test_steering_pid.c diff --git a/include/jlink.h b/include/jlink.h index 2eb5f32..c3f03de 100644 --- a/include/jlink.h +++ b/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 */ diff --git a/include/mpu6000.h b/include/mpu6000.h index b1455ad..d182037 100644 --- a/include/mpu6000.h +++ b/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; diff --git a/include/steering_pid.h b/include/steering_pid.h new file mode 100644 index 0000000..66ef413 --- /dev/null +++ b/include/steering_pid.h @@ -0,0 +1,134 @@ +#ifndef STEERING_PID_H +#define STEERING_PID_H + +#include +#include + +/* + * 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 */ diff --git a/src/jlink.c b/src/jlink.c index 89ab878..2b3156e 100644 --- a/src/jlink.c +++ b/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)); +} diff --git a/src/mpu6000.c b/src/mpu6000.c index 215c5b4..831907f 100644 --- a/src/mpu6000.c +++ b/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; } diff --git a/src/steering_pid.c b/src/steering_pid.c new file mode 100644 index 0000000..825bfbd --- /dev/null +++ b/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); +} diff --git a/test/test_steering_pid.c b/test/test_steering_pid.c new file mode 100644 index 0000000..0e2bb83 --- /dev/null +++ b/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 +#include + +/* 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 +#include +#include + +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; +} -- 2.47.2