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/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; +}