seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

fix: Status LEDs solid=OK blink=error (#22) #26

Merged
seb merged 2 commits from sl-firmware/status-leds into main 2026-02-28 18:45:24 -05:00
Conversation 1 Commits 2 Files Changed 6 +110 -29
6 changed files with 110 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/config.h
View File

@ -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

12
include/mpu6000.h
View File

@ -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

14
include/status.h
View File

@ -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

20
src/main.c
View File

@ -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);
}
}

51
src/mpu6000.c
View File

@ -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.

39
src/status.c
View File

@ -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);
}
}