From f02ed8172a68746082587c19e6273b9635ec86ca Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sat, 28 Feb 2026 17:42:24 -0500 Subject: [PATCH 1/2] =?UTF-8?q?feat:=20gyro=20bias=20calibration=20on=20bo?= =?UTF-8?q?ot=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. From 011f212056274ec5431376432befed9b85e70abc Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Sat, 28 Feb 2026 17:43:25 -0500 Subject: [PATCH 2/2] fix: Status LEDs solid=OK blink=error (#22) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit New LED behavior (active-low, PC15=LED1, PC14=LED2): Disarmed, IMU OK : LED1 solid ON + LED2 off Armed : LED1 solid ON + LED2 solid ON Tilt fault : LED1 blink 1Hz + LED2 blink 1Hz IMU error : LED1 blink 1Hz + LED2 solid ON Rule: solid = good, slow blink (~1Hz) = needs attention. Removed the confusing fast-blink-at-5Hz-for-error and the baro-flash-every-5s patterns. status_update() signature changed: baro_ok → armed + tilt_fault so the LED pattern can reflect arm state directly. Closes #22. Co-Authored-By: Claude Sonnet 4.6 --- include/status.h | 14 +++++++++++++- src/main.c | 4 +++- src/status.c | 39 +++++++++++++++++++-------------------- 3 files changed, 35 insertions(+), 22 deletions(-) diff --git a/include/status.h b/include/status.h index 9badbb6..6519b1a 100644 --- a/include/status.h +++ b/include/status.h @@ -3,5 +3,17 @@ #include void status_init(void); void status_boot_beep(void); -void status_update(uint32_t tick, int imu_ok, int baro_ok); +/* + * status_update() — call every main loop iteration. + * Controls LED1 (PC15) and LED2 (PC14), both active-low. + * + * Solid ON = good (normal operation) + * Slow blink (~1 Hz) = needs attention (error or fault) + * + * LED1 solid + LED2 off → disarmed, IMU OK + * LED1 solid + LED2 solid → armed + * Both slow blink → tilt fault + * LED1 slow blink + LED2 solid → IMU error (solid LED2 = always-on indicator) + */ +void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault); #endif diff --git a/src/main.c b/src/main.c index 38e0ba1..be4f77c 100644 --- a/src/main.c +++ b/src/main.c @@ -232,7 +232,9 @@ int main(void) { CDC_Transmit((uint8_t *)buf, len); } - status_update(now, (imu_ret == 0), (bal.state == BALANCE_ARMED)); + status_update(now, (imu_ret == 0), + (bal.state == BALANCE_ARMED), + (bal.state == BALANCE_TILT_FAULT)); HAL_Delay(1); } } diff --git a/src/status.c b/src/status.c index 58dd31b..4045220 100644 --- a/src/status.c +++ b/src/status.c @@ -42,26 +42,25 @@ void status_boot_beep(void) { HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); } -void status_update(uint32_t tick, int imu_ok, int baro_ok) { - if (imu_ok) { - /* Slow blink LED1 at 1Hz */ - if ((tick / 500) % 2) - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); /* ON */ - else - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_SET); /* OFF */ - - /* LED2: quick flash every 5s if baro OK */ - if (baro_ok && (tick % 5000) < 100) - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); - else - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET); - } else { - /* Fast blink LED1 at 5Hz = error */ - if ((tick / 100) % 2) - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); - else - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_SET); - /* LED2 solid ON */ +void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault) { + /* Solid = good, blink = needs attention (1 Hz, 500ms half-period) */ + GPIO_PinState blink = ((tick / 500) % 2) ? GPIO_PIN_RESET /* ON half */ + : GPIO_PIN_SET; /* OFF half */ + if (!imu_ok) { + /* IMU error: LED1 blinking (attention), LED2 solid ON */ + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink); HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); + } else if (tilt_fault) { + /* Tilt fault: both LEDs slow blink */ + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink); + HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, blink); + } else if (armed) { + /* Armed: both LEDs solid ON */ + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); + } else { + /* Normal disarmed: LED1 solid ON, LED2 off */ + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET); } }