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/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 45516c1..be4f77c 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; @@ -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); } } 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. 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); } }