saltylab-firmware/include/steering_pid.h

#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 */