sl-controls
1e69337ffd
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 <noreply@anthropic.com>
33 lines
970 B
C
33 lines
970 B
C
#ifndef MPU6000_H
|
|
#define MPU6000_H
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
|
|
typedef struct {
|
|
float pitch; // degrees, filtered (complementary filter)
|
|
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;
|
|
|
|
bool mpu6000_init(void);
|
|
|
|
/*
|
|
* Sample gyro for ~1s to compute per-axis bias offsets.
|
|
* Must be called after mpu6000_init() with the board held still.
|
|
* Blocks ~1s. LED1+LED2 solid during calibration.
|
|
* IWDG must NOT be running when this is called (call before safety_init()).
|
|
*/
|
|
void mpu6000_calibrate(void);
|
|
|
|
/* Returns true once mpu6000_calibrate() has completed. */
|
|
bool mpu6000_is_calibrated(void);
|
|
|
|
void mpu6000_read(IMUData *data);
|
|
|
|
#endif
|