From 1c95243e520f1e1fd245d7296636d7303082fd40 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sat, 28 Feb 2026 17:42:24 -0500 Subject: [PATCH] =?UTF-8?q?feat:=20gyro=20bias=20calibration=20on=20boot?= =?UTF-8?q?=20=E2=80=94=20fixes=20yaw=20drift=20(issues=20#21,=20#23)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit On boot, before the main loop, sample 1000 gyro readings (~1s) while board is held still. Compute per-axis mean offset (sensor-frame raw LSBs) and subtract from all subsequent readings in mpu6000_read(). - mpu6000_calibrate(): LED1+LED2 solid ON during 1s sample window, resets filter state to zero once bias is known - mpu6000_is_calibrated(): gate; main loop blocks arming and USB streaming until calibration completes - Bias subtracted in sensor frame before CW270 axis transform + scale, so all three axes (pitch/roll/yaw rate) benefit - config.h: GYRO_CAL_SAMPLES=1000 - No flash storage — recalibrate fresh each boot (bias varies with temp) Closes #21 (3.5°/s yaw drift), #23 (gyro bias calibration on boot). Co-Authored-By: Claude Sonnet 4.6 --- include/config.h | 3 +++ include/mpu6000.h | 12 +++++++++++ src/main.c | 16 +++++++++++---- src/mpu6000.c | 51 ++++++++++++++++++++++++++++++++++++++++++++--- 4 files changed, 75 insertions(+), 7 deletions(-) diff --git a/include/config.h b/include/config.h index 293fe7e..b05641f 100644 --- a/include/config.h +++ b/include/config.h @@ -144,4 +144,7 @@ #define MOTOR_CMD_MAX 1000 /* ESC range: -1000..+1000 */ #define MOTOR_STEER_RAMP_RATE 20 /* counts/ms — steer ramp only */ +// --- IMU Calibration --- +#define GYRO_CAL_SAMPLES 1000 /* gyro bias samples (~1s at 1ms/sample) */ + #endif // CONFIG_H diff --git a/include/mpu6000.h b/include/mpu6000.h index b19ea45..b1455ad 100644 --- a/include/mpu6000.h +++ b/include/mpu6000.h @@ -14,6 +14,18 @@ typedef struct { } IMUData; bool mpu6000_init(void); + +/* + * Sample gyro for ~1s to compute per-axis bias offsets. + * Must be called after mpu6000_init() with the board held still. + * Blocks ~1s. LED1+LED2 solid during calibration. + * IWDG must NOT be running when this is called (call before safety_init()). + */ +void mpu6000_calibrate(void); + +/* Returns true once mpu6000_calibrate() has completed. */ +bool mpu6000_is_calibrated(void); + void mpu6000_read(IMUData *data); #endif diff --git a/src/main.c b/src/main.c index 45516c1..38e0ba1 100644 --- a/src/main.c +++ b/src/main.c @@ -108,6 +108,13 @@ int main(void) { /* Init IMU (MPU6000 via SPI1 — mpu6000.c wraps icm42688 + complementary filter) */ int imu_ret = mpu6000_init() ? 0 : -1; + /* + * Gyro bias calibration — hold board still for ~1s. + * Must run before safety_init() (IWDG not yet started). + * Blocks arming and streaming until complete. + */ + if (imu_ret == 0) mpu6000_calibrate(); + /* Init hoverboard ESC UART */ hoverboard_init(); @@ -156,8 +163,9 @@ int main(void) { /* Handle arm/disarm requests from USB with arm-hold interlock */ if (cdc_arm_request) { cdc_arm_request = 0; - /* Start the arm hold timer — motors don't enable until ARMING_HOLD_MS */ - if (bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) { + /* Block arming until gyro calibration is complete */ + if (mpu6000_is_calibrated() && + bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) { safety_arm_start(now); } } @@ -204,8 +212,8 @@ int main(void) { } } - /* USB telemetry at 50Hz (only when streaming enabled via S command) */ - if (cdc_streaming && now - send_tick >= 20) { + /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */ + if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) { send_tick = now; if (imu_ret == 0) { float err = bal.setpoint - bal.pitch_deg; diff --git a/src/mpu6000.c b/src/mpu6000.c index 6a37165..443e671 100644 --- a/src/mpu6000.c +++ b/src/mpu6000.c @@ -11,6 +11,7 @@ #include "mpu6000.h" #include "icm42688.h" +#include "config.h" #include "stm32f7xx_hal.h" #include @@ -31,6 +32,12 @@ static float s_roll = 0.0f; static float s_yaw = 0.0f; static uint32_t s_last_tick = 0; +/* Gyro bias offsets (raw sensor LSBs, sensor frame) */ +static float s_bias_gx = 0.0f; +static float s_bias_gy = 0.0f; +static float s_bias_gz = 0.0f; +static bool s_calibrated = false; + bool mpu6000_init(void) { int ret = icm42688_init(); if (ret == 0) { @@ -42,6 +49,43 @@ bool mpu6000_init(void) { return (ret == 0); } +void mpu6000_calibrate(void) { + /* LED1 + LED2 solid ON (active low) — visual indicator: calibrating */ + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); + + /* + * Accumulate raw gyro readings in sensor frame. + * Use int32 to avoid float accumulation error over 1000 samples. + * Max raw value ~32768, 1000 samples → max sum ~32.7M < INT32_MAX. + */ + int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0; + for (int i = 0; i < GYRO_CAL_SAMPLES; i++) { + icm42688_data_t raw; + icm42688_read(&raw); + sum_gx += raw.gx; + sum_gy += raw.gy; + sum_gz += raw.gz; + HAL_Delay(1); + } + + s_bias_gx = (float)sum_gx / GYRO_CAL_SAMPLES; + s_bias_gy = (float)sum_gy / GYRO_CAL_SAMPLES; + s_bias_gz = (float)sum_gz / GYRO_CAL_SAMPLES; + + /* Reset filter state so angles start from zero with clean bias */ + s_pitch = 0.0f; + s_roll = 0.0f; + s_yaw = 0.0f; + s_last_tick = HAL_GetTick(); + + s_calibrated = true; +} + +bool mpu6000_is_calibrated(void) { + return s_calibrated; +} + void mpu6000_read(IMUData *data) { icm42688_data_t raw; icm42688_read(&raw); @@ -71,9 +115,10 @@ void mpu6000_read(IMUData *data) { * * Convention: pitch+ = nose up, roll+ = right bank, yaw+ = CW from above. */ - float gyro_pitch_rate = -raw.gx * GYRO_SCALE; /* °/s board_gy = -sensor_gx */ - float gyro_roll_rate = raw.gy * GYRO_SCALE; /* °/s board_gx = sensor_gy */ - float gyro_yaw_rate = raw.gz * GYRO_SCALE; /* °/s unchanged */ + /* Subtract bias (sensor-frame raw LSBs) before axis transform + scale */ + float gyro_pitch_rate = -(raw.gx - s_bias_gx) * GYRO_SCALE; /* °/s board_gy = -sensor_gx */ + float gyro_roll_rate = (raw.gy - s_bias_gy) * GYRO_SCALE; /* °/s board_gx = sensor_gy */ + float gyro_yaw_rate = (raw.gz - s_bias_gz) * GYRO_SCALE; /* °/s unchanged */ /* * Accel-derived angles after CW270 transform. -- 2.47.2