From 6dfb5640db3357d21a7864a0d2ba2296ceca0576 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sat, 28 Feb 2026 12:26:28 -0500 Subject: [PATCH] feat(imu): MPU6000 sensor fusion with complementary filter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- include/balance.h | 6 ++-- src/balance.c | 37 ++++----------------- src/main.c | 20 ++++++----- src/mpu6000.c | 85 +++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 105 insertions(+), 43 deletions(-) create mode 100644 src/mpu6000.c diff --git a/include/balance.h b/include/balance.h index ac30a20..3f6a81e 100644 --- a/include/balance.h +++ b/include/balance.h @@ -2,12 +2,12 @@ #define BALANCE_H #include -#include "icm42688.h" +#include "mpu6000.h" /* * SaltyLab Balance Controller * - * Complementary filter (gyro + accel) → pitch angle + * Consumes fused IMUData (pitch + pitch_rate from mpu6000 complementary filter) * PID controller → motor speed command * Safety: tilt cutoff, arming, watchdog */ @@ -39,7 +39,7 @@ typedef struct { } balance_t; void balance_init(balance_t *b); -void balance_update(balance_t *b, const icm42688_data_t *imu, float dt); +void balance_update(balance_t *b, const IMUData *imu, float dt); void balance_arm(balance_t *b); void balance_disarm(balance_t *b); diff --git a/src/balance.c b/src/balance.c index 8659c0b..4cb9ff7 100644 --- a/src/balance.c +++ b/src/balance.c @@ -2,13 +2,6 @@ #include "config.h" #include -/* MPU6000 raw to physical units (default ±2g, ±250°/s) */ -#define ACCEL_SCALE (1.0f / 16384.0f) /* LSB to g (±2g range) */ -#define GYRO_SCALE (1.0f / 131.0f) /* LSB to °/s (±250°/s range) */ - -/* Complementary filter coefficient — 0.98 trusts gyro, 0.02 corrects with accel */ -#define COMP_ALPHA 0.98f - void balance_init(balance_t *b) { b->state = BALANCE_DISARMED; b->pitch_deg = 0.0f; @@ -28,30 +21,12 @@ void balance_init(balance_t *b) { b->max_speed = MAX_SPEED_LIMIT; } -void balance_update(balance_t *b, const icm42688_data_t *imu, float dt) { +void balance_update(balance_t *b, const IMUData *imu, float dt) { if (dt <= 0.0f || dt > 0.1f) return; /* Sanity check dt */ - /* Convert raw IMU to physical units */ - float ax = imu->ax * ACCEL_SCALE; - float ay = imu->ay * ACCEL_SCALE; - float az = imu->az * ACCEL_SCALE; - - /* - * Gyro axis for pitch depends on mounting orientation. - * MPU6000 on MAMBA F722S with CW270 alignment: - * Pitch rate = gx axis (adjust sign if needed during testing) - */ - float gyro_pitch_rate = imu->gx * GYRO_SCALE; - b->pitch_rate = gyro_pitch_rate; - - /* Accelerometer pitch angle (atan2 of forward/down axes) - * With CW270: pitch = atan2(ax, az) - * Adjust axes based on actual mounting during testing */ - float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265f); - - /* Complementary filter */ - b->pitch_deg = COMP_ALPHA * (b->pitch_deg + gyro_pitch_rate * dt) - + (1.0f - COMP_ALPHA) * accel_pitch; + /* Consume fused angle from mpu6000 complementary filter */ + b->pitch_deg = imu->pitch; + b->pitch_rate = imu->pitch_rate; /* Safety: tilt cutoff */ if (b->state == BALANCE_ARMED) { @@ -82,8 +57,8 @@ void balance_update(balance_t *b, const icm42688_data_t *imu, float dt) { if (b->integral < -PID_INTEGRAL_MAX) b->integral = -PID_INTEGRAL_MAX; float i_term = b->ki * b->integral; - /* Derivative (on measurement to avoid setpoint kick) */ - float d_term = b->kd * (-gyro_pitch_rate); /* Use gyro directly, cleaner than differencing */ + /* Derivative on measurement (avoids setpoint kick) */ + float d_term = b->kd * (-b->pitch_rate); /* Sum and clamp */ float output = p_term + i_term + d_term; diff --git a/src/main.c b/src/main.c index d359c20..15cc82a 100644 --- a/src/main.c +++ b/src/main.c @@ -3,7 +3,7 @@ #include "usbd_cdc.h" #include "usbd_cdc_if.h" #include "usbd_desc.h" -#include "icm42688.h" +#include "mpu6000.h" #include "bmp280.h" #include "balance.h" #include "hoverboard.h" @@ -67,8 +67,9 @@ int main(void) { status_init(); HAL_Delay(3000); /* Wait for USB host to enumerate */ - /* Init IMU */ - int imu_ret = icm42688_init(); + /* Init IMU (MPU6000 via SPI1, complementary filter in mpu6000.c) */ + bool imu_ok = mpu6000_init(); + int imu_ret = imu_ok ? 0 : -1; /* Init hoverboard ESC UART */ hoverboard_init(); @@ -80,14 +81,14 @@ int main(void) { char buf[256]; int len; - icm42688_data_t imu; + IMUData imu; uint32_t send_tick = 0; uint32_t balance_tick = 0; uint32_t esc_tick = 0; const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */ while (1) { - if (imu_ret == 0) icm42688_read(&imu); + if (imu_ret == 0) mpu6000_read(&imu); uint32_t now = HAL_GetTick(); @@ -123,10 +124,11 @@ int main(void) { send_tick = now; if (imu_ret == 0) { len = snprintf(buf, sizeof(buf), - "{\"ax\":%d,\"ay\":%d,\"az\":%d,\"gx\":%d,\"gy\":%d,\"gz\":%d," - "\"p\":%d,\"m\":%d,\"s\":%d}\n", - imu.ax, imu.ay, imu.az, imu.gx, imu.gy, imu.gz, - (int)(bal.pitch_deg * 10), /* pitch x10 for precision without %f */ + "{\"ax\":%d,\"az\":%d,\"r\":%d,\"p\":%d,\"m\":%d,\"s\":%d}\n", + (int)(imu.accel_x * 1000), /* g x1000 */ + (int)(imu.accel_z * 1000), /* g x1000 */ + (int)(imu.pitch_rate * 10), /* °/s x10 */ + (int)(bal.pitch_deg * 10), /* degrees x10 */ (int)bal.motor_cmd, (int)bal.state); } else { diff --git a/src/mpu6000.c b/src/mpu6000.c new file mode 100644 index 0000000..9e700d4 --- /dev/null +++ b/src/mpu6000.c @@ -0,0 +1,85 @@ +/* + * mpu6000.c — IMU Sensor Fusion for MPU6000 + * + * Wraps the icm42688 SPI driver (which auto-detects MPU6000) and applies + * a complementary filter to produce a stable pitch estimate. + * + * Hardware: MAMBA F722S, MPU6000 on SPI1, CW270 alignment + * Config: Gyro ±2000°/s (init_mpu6000 sets FS_SEL=3) + * Accel ±16g (init_mpu6000 sets AFS_SEL=3) + */ + +#include "mpu6000.h" +#include "icm42688.h" +#include "stm32f7xx_hal.h" +#include + +/* Scale factors matching init_mpu6000() config in icm42688.c */ +#define GYRO_SCALE (1.0f / 16.384f) /* LSB to °/s — ±2000°/s range */ +#define ACCEL_SCALE (1.0f / 2048.0f) /* LSB to g — ±16g range */ + +/* + * Complementary filter coefficient. + * 0.98 = trust gyro integration, 0.02 = accel correction for drift. + * Tune higher (0.99) for noisier environments. + */ +#define COMP_ALPHA 0.98f + +/* Filter state */ +static float s_pitch = 0.0f; +static uint32_t s_last_tick = 0; + +bool mpu6000_init(void) { + int ret = icm42688_init(); + if (ret == 0) { + s_pitch = 0.0f; + s_last_tick = HAL_GetTick(); + } + return (ret == 0); +} + +void mpu6000_read(IMUData *data) { + icm42688_data_t raw; + icm42688_read(&raw); + + /* Compute dt from wall clock — robust to loop jitter */ + uint32_t now = HAL_GetTick(); + uint32_t elapsed_ms = now - s_last_tick; + if (elapsed_ms == 0) elapsed_ms = 1; /* min 1ms to avoid divide-by-zero */ + if (elapsed_ms > 100) elapsed_ms = 100; /* clamp: don't integrate stale data */ + float dt = elapsed_ms * 0.001f; + s_last_tick = now; + + /* Convert raw to physical units */ + float ax = raw.ax * ACCEL_SCALE; /* g */ + float az = raw.az * ACCEL_SCALE; /* g */ + + /* + * Gyro pitch axis with CW270 alignment: pitch rate = gx. + * Sign may need inverting depending on mounting orientation — + * verify during bench test (positive nose-up should be positive). + */ + float gyro_pitch_rate = raw.gx * GYRO_SCALE; /* °/s */ + + /* + * Accel pitch angle (degrees). + * CW270 alignment: pitch = atan2(ax, az). + * Valid while ax² + az² ≈ 1g (i.e., low linear acceleration). + */ + float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265358979f); + + /* + * Complementary filter: + * pitch = α * (pitch + ω*dt) + (1−α) * accel_pitch + * + * Gyro integration tracks fast dynamics; accel correction + * prevents long-term drift. + */ + s_pitch = COMP_ALPHA * (s_pitch + gyro_pitch_rate * dt) + + (1.0f - COMP_ALPHA) * accel_pitch; + + data->pitch = s_pitch; + data->pitch_rate = gyro_pitch_rate; + data->accel_x = ax; + data->accel_z = az; +}