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

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

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;

src/mpu6000.c

@@ -11,6 +11,7 @@
 
 #include "mpu6000.h"
 #include "icm42688.h"
+#include "config.h"
 #include "stm32f7xx_hal.h"
 #include <math.h>
 
@@ -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.