Compare commits

..

No commits in common. "e1b82941ea30a2d2c972330806d22314dbec7ce8" and "1bf30a0262cd11ea9d88f00c2faa693ca6ca58b4" have entirely different histories.

3 changed files with 22 additions and 35 deletions

View File

@ -3,17 +3,5 @@
#include <stdint.h> #include <stdint.h>
void status_init(void); void status_init(void);
void status_boot_beep(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 #endif

View File

@ -232,9 +232,7 @@ int main(void) {
CDC_Transmit((uint8_t *)buf, len); CDC_Transmit((uint8_t *)buf, len);
} }
status_update(now, (imu_ret == 0), status_update(now, (imu_ret == 0), (bal.state == BALANCE_ARMED));
(bal.state == BALANCE_ARMED),
(bal.state == BALANCE_TILT_FAULT));
HAL_Delay(1); HAL_Delay(1);
} }
} }

View File

@ -42,25 +42,26 @@ void status_boot_beep(void) {
HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
} }
void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault) { void status_update(uint32_t tick, int imu_ok, int baro_ok) {
/* Solid = good, blink = needs attention (1 Hz, 500ms half-period) */ if (imu_ok) {
GPIO_PinState blink = ((tick / 500) % 2) ? GPIO_PIN_RESET /* ON half */ /* Slow blink LED1 at 1Hz */
: GPIO_PIN_SET; /* OFF half */ if ((tick / 500) % 2)
if (!imu_ok) { HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); /* ON */
/* IMU error: LED1 blinking (attention), LED2 solid ON */ else
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink); 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); HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);
} else if (tilt_fault) { else
/* 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); 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 */
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);
} }
} }