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

include/balance.h

@@ -2,12 +2,12 @@
 #define BALANCE_H
 
 #include <stdint.h>
-#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);
 

src/balance.c

@@ -2,13 +2,6 @@
 #include "config.h"
 #include <math.h>
 
-/* 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 */
+    /* Consume fused angle from mpu6000 complementary filter */
-    float ax = imu->ax * ACCEL_SCALE;
+    b->pitch_deg  = imu->pitch;
-    float ay = imu->ay * ACCEL_SCALE;
+    b->pitch_rate = imu->pitch_rate;
-    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;
 
     /* 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) */
+    /* Derivative on measurement (avoids setpoint kick) */
-    float d_term = b->kd * (-gyro_pitch_rate);  /* Use gyro directly, cleaner than differencing */
+    float d_term = b->kd * (-b->pitch_rate);
 
     /* Sum and clamp */
     float output = p_term + i_term + d_term;

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 <math.h>
+
+/* 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;
+}