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

include/status.h

@@ -3,5 +3,17 @@
 #include <stdint.h>
 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

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 */
+            /* Block arming until gyro calibration is complete */
-            if (bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
+            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) */
+        /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
-        if (cdc_streaming && now - send_tick >= 20) {
+        if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
             send_tick = now;
             if (imu_ret == 0) {
                 float err = bal.setpoint - bal.pitch_deg;
@@ -224,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);
     }
 }

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 */
+    /* Subtract bias (sensor-frame raw LSBs) before axis transform + scale */
-    float gyro_roll_rate  =  raw.gy * GYRO_SCALE;  /* °/s  board_gx =  sensor_gy */
+    float gyro_pitch_rate = -(raw.gx - s_bias_gx) * GYRO_SCALE;  /* °/s  board_gy = -sensor_gx */
-    float gyro_yaw_rate   =  raw.gz * GYRO_SCALE;  /* °/s  unchanged */
+    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.

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) {
+void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault) {
-    if (imu_ok) {
+    /* Solid = good, blink = needs attention (1 Hz, 500ms half-period) */
-        /* Slow blink LED1 at 1Hz */
+    GPIO_PinState blink = ((tick / 500) % 2) ? GPIO_PIN_RESET  /* ON half */
-        if ((tick / 500) % 2)
+                                              : GPIO_PIN_SET;   /* OFF half */
-            HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); /* ON */
+    if (!imu_ok) {
-        else
+        /* IMU error: LED1 blinking (attention), LED2 solid ON */
-            HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_SET);   /* OFF */
+        HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink);
-
-        /* 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
+    } else if (tilt_fault) {
-            HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET);
+        /* Tilt fault: both LEDs slow blink */
-    } else {
+        HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink);
-        /* Fast blink LED1 at 5Hz = error */
+        HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, blink);
-        if ((tick / 100) % 2)
+    } else if (armed) {
+        /* Armed: both LEDs solid ON */
         HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET);
-        else
-            HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_SET);
-        /* LED2 solid ON */
         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);
     }
 }