feat(imu): MPU6000 sensor fusion — complementary filter (bd-2dv) #2

sl-controls · 2026-02-28T12:28:31-05:00

sl-controls commented

2026-02-28 12:28:31 -05:00

Summary

Add src/mpu6000.c implementing a complementary filter (α=0.98) over the existing icm42688 SPI driver
Fix wrong scale factors in balance.c (was ±250°/s/±2g; hardware is configured ±2000°/s/±16g)
Move sensor fusion out of balance_update() into the IMU driver layer — balance controller now consumes IMUData.pitch directly
Update main.c to use mpu6000_init()/mpu6000_read() and IMUData
Update telemetry to report physical units (g × 1000, °/s × 10)

Filter design

pitch = 0.98 × (pitch + ω·dt) + 0.02 × atan2(ax, az)

Dt is computed from HAL_GetTick() on each read — robust to loop jitter.

Testing notes

Gyro pitch axis (gx with CW270 align) and sign may need verification on bench
COMP_ALPHA tunable in mpu6000.c (increase toward 0.99 for noisier environments)

Closes bd-2dv

## Summary - Add `src/mpu6000.c` implementing a complementary filter (α=0.98) over the existing icm42688 SPI driver - Fix wrong scale factors in `balance.c` (was ±250°/s/±2g; hardware is configured ±2000°/s/±16g) - Move sensor fusion out of `balance_update()` into the IMU driver layer — balance controller now consumes `IMUData.pitch` directly - Update `main.c` to use `mpu6000_init()`/`mpu6000_read()` and `IMUData` - Update telemetry to report physical units (g × 1000, °/s × 10) ## Filter design ``` pitch = 0.98 × (pitch + ω·dt) + 0.02 × atan2(ax, az) ``` Dt is computed from `HAL_GetTick()` on each read — robust to loop jitter. ## Testing notes - Gyro pitch axis (gx with CW270 align) and sign may need verification on bench - `COMP_ALPHA` tunable in `mpu6000.c` (increase toward 0.99 for noisier environments) Closes bd-2dv

sl-controls added 1 commit 2026-02-28 12:28:32 -05:00

feat(imu): MPU6000 sensor fusion with complementary filter 6dfb5640db

Add src/mpu6000.c implementing a complementary filter (α=0.98) on top of
the existing icm42688 SPI driver. Fixes wrong scale factors in balance.c
(was ±250°/s / ±2g; hardware is configured ±2000°/s / ±16g). Fusion now
lives in the IMU driver layer; balance_update() consumes IMUData directly.

- mpu6000_init(): calls icm42688_init(), seeds filter state
- mpu6000_read(): reads raw SPI, applies complementary filter, returns
  fused pitch (degrees) + pitch_rate (°/s) + accel_x/z (g)
- balance.c: removes duplicated fusion code, uses IMUData.pitch
- main.c: switches to mpu6000_init()/mpu6000_read(), updates telemetry

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

sl-controls referenced this pull request

2026-02-28 12:31:03 -05:00

feat(pid): runtime PID tuning via USB + telemetry (bd-18i) #3

sl-controls force-pushed sl-controls/bd-2dv-imu-fusion from 6dfb5640db to 398cbb9a55

2026-02-28 13:09:29 -05:00

Compare

sl-jetson merged commit 098475a606 into main

2026-02-28 13:12:10 -05:00

sl-jetson referenced this issue from a commit

2026-02-28 13:12:10 -05:00

Merge pull request 'feat(imu): MPU6000 sensor fusion — complementary filter (bd-2dv)' (#2) from sl-controls/bd-2dv-imu-fusion into main

Sign in to join this conversation.