sl-mechanical a2c554c232 cleanup: remove all Mamba/F722S/STM32F722 refs — replace with ESP32-S3 BALANCE/IO
- docs/: rewrite AGENTS.md, wiring-diagram.md (SAUL-TEE arch); update
  SALTYLAB.md, FACE_LCD_ANIMATION.md, board-viz.html, SALTYLAB-DETAILED refs
- cad/: dimensions.scad FC params → ESP32-S3 BALANCE params
- chassis/: ASSEMBLY.md, BOM.md, ip54_BOM.md, *.scad — FC_MOUNT_SPACING/
  FC_PITCH → TBD ESP32-S3; Drone FC → MCU mount throughout
- CLAUDE.md, TEAM.md: project desc → SAUL-TEE; hardware table → ESP32-S3/VESC
- USB_CDC_BUG.md: marked ARCHIVED (legacy STM32 era)
- AUTONOMOUS_ARMING.md: USB CDC → inter-board UART (ESP32-S3 BALANCE)
- projects/saltybot/SLAM-SETUP-PLAN.md: FC/STM32F722 → BALANCE/CAN
- jetson/docs/pinout.md, power-budget.md, README.md: STM32 bridge → CAN bridge
- jetson/config/RECOVERY_BEHAVIORS.md: FC+Hoverboard → BALANCE+VESC
- jetson/ros2_ws: stm32_protocol.py → esp32_protocol.py,
  stm32_cmd_node.py → esp32_cmd_node.py,
  mamba_protocol.py → balance_protocol.py; can_bridge_node imports updated
- scripts/flash_firmware.py: DFU/STM32 → pio run -t upload
- src/ include/: ARCHIVED headers added (legacy code preserved)
- test/: ARCHIVED notices; STM32F722 comments marked LEGACY
- ui/diagnostics_panel.html: Board/STM32 → ESP32-S3

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-04 09:06:09 -04:00

136 lines
5.4 KiB
C

/* ARCHIVED: Legacy STM32F722/Mamba F722S era code. NOT used in current hardware. */
#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 */