seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat: Steering PID controller (Issue #616) #624

Merged
sl-jetson merged 1 commits from sl-controls/issue-616-steering-pid into main 2026-03-15 11:02:33 -04:00
Conversation 0 Commits 1 Files Changed 7 +953
sl-perception commented 2026-03-15 10:11:36 -04:00
Collaborator
Copy Link

Implements Issue #616: closed-loop yaw-rate PID controller for differential drive that converts Jetson Twist.angular.z to differential wheel speed offsets using IMU gyro Z feedback.

Changes

New: include/steering_pid.h + src/steering_pid.c

  • PID with Kp=2.0, Ki=0.5, Kd=0.05
  • Anti-windup: integral clamped to ±200 counts before Ki multiply
  • Rate limiter: max 10 counts/ms output change — protects balance PID
  • STEER_OMEGA_SCALE = 0.1 deg/s/unit: steer ±1000 → ±100 deg/s
  • STEER_OUTPUT_MAX = 400 (matches CRSF_STEER_MAX)
  • JLINK_TLM_STEERING (0x8A) at 10 Hz

Modified: include/mpu6000.h + src/mpu6000.c

  • Added yaw_rate field to IMUData (bias-corrected board_gz, deg/s)

Modified: include/jlink.h + src/jlink.c

  • Added JLINK_TLM_STEERING 0x8Au, jlink_tlm_steering_t (8 bytes), jlink_send_steering_tlm()

New: test/test_steering_pid.c

  • 78 unit tests, all passing

Closes #616

Implements Issue #616: closed-loop yaw-rate PID controller for differential drive that converts Jetson Twist.angular.z to differential wheel speed offsets using IMU gyro Z feedback. ## Changes ### New: `include/steering_pid.h` + `src/steering_pid.c` - PID with Kp=2.0, Ki=0.5, Kd=0.05 - Anti-windup: integral clamped to ±200 counts before Ki multiply - Rate limiter: max 10 counts/ms output change — protects balance PID - STEER_OMEGA_SCALE = 0.1 deg/s/unit: steer ±1000 → ±100 deg/s - STEER_OUTPUT_MAX = 400 (matches CRSF_STEER_MAX) - JLINK_TLM_STEERING (0x8A) at 10 Hz ### Modified: `include/mpu6000.h` + `src/mpu6000.c` - Added yaw_rate field to IMUData (bias-corrected board_gz, deg/s) ### Modified: `include/jlink.h` + `src/jlink.c` - Added JLINK_TLM_STEERING 0x8Au, jlink_tlm_steering_t (8 bytes), jlink_send_steering_tlm() ### New: `test/test_steering_pid.c` - 78 unit tests, all passing Closes #616
sl-perception added 1 commit 2026-03-15 10:11:36 -04:00 
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>
sl-jetson merged commit 13b17a11e1 into main 2026-03-15 11:02:33 -04:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#624
No description provided.
Delete Branch "sl-controls/issue-616-steering-pid"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?