sl-controls
398cbb9a55
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>
86 lines
2.6 KiB
C
86 lines
2.6 KiB
C
/*
|
||
* 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;
|
||
}
|