Compare commits
No commits in common. "b3edabc9c59d0961fdeeefa25715f80abc478929" and "b087af4b947f52adc025ae595123c7d3339378b3" have entirely different histories.
b3edabc9c5
...
b087af4b94
@ -1,124 +0,0 @@
|
|||||||
#ifndef RGB_FSM_H
|
|
||||||
#define RGB_FSM_H
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
|
|
||||||
/*
|
|
||||||
* rgb_fsm.h — RGB Status LED State Machine (Issue #290)
|
|
||||||
*
|
|
||||||
* Manages an 8-LED WS2812 NeoPixel ring with 8 operational states.
|
|
||||||
* Each state has a specific color pattern and animation.
|
|
||||||
*
|
|
||||||
* States:
|
|
||||||
* BOOT — Blue pulse (startup sequence, 0.5 Hz)
|
|
||||||
* IDLE — Green breathe (standby, smooth 0.5 Hz pulse)
|
|
||||||
* ARMED — Solid green (ready to move)
|
|
||||||
* NAV — Cyan spin (autonomous navigation active, rotating pattern)
|
|
||||||
* ERROR — Red flash (fault detected, 2 Hz blink)
|
|
||||||
* LOW_BATT — Orange blink (battery low, 1 Hz blink)
|
|
||||||
* CHARGING — Green fill (charging, progressive LEDs filling)
|
|
||||||
* ESTOP — Red solid (emergency stop, full red, no animation)
|
|
||||||
*
|
|
||||||
* Transitions via UART command from Jetson.
|
|
||||||
* Non-blocking operation with tick-based timing.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* LED State Machine States */
|
|
||||||
typedef enum {
|
|
||||||
LED_STATE_BOOT = 0,
|
|
||||||
LED_STATE_IDLE,
|
|
||||||
LED_STATE_ARMED,
|
|
||||||
LED_STATE_NAV,
|
|
||||||
LED_STATE_ERROR,
|
|
||||||
LED_STATE_LOW_BATT,
|
|
||||||
LED_STATE_CHARGING,
|
|
||||||
LED_STATE_ESTOP,
|
|
||||||
LED_STATE_COUNT
|
|
||||||
} LedState;
|
|
||||||
|
|
||||||
/* RGB Color (8-bit per channel) */
|
|
||||||
typedef struct {
|
|
||||||
uint8_t r; /* Red (0-255) */
|
|
||||||
uint8_t g; /* Green (0-255) */
|
|
||||||
uint8_t b; /* Blue (0-255) */
|
|
||||||
} RgbColor;
|
|
||||||
|
|
||||||
/*
|
|
||||||
* rgb_fsm_init()
|
|
||||||
*
|
|
||||||
* Initialize LED state machine:
|
|
||||||
* - PB4 as TIM3_CH1 PWM output for WS2812 driver
|
|
||||||
* - Configure TIM3 for 800 kHz PWM frequency
|
|
||||||
* - Set initial state to BOOT
|
|
||||||
* - Initialize all LEDs to off
|
|
||||||
*/
|
|
||||||
void rgb_fsm_init(void);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* rgb_fsm_set_state(state)
|
|
||||||
*
|
|
||||||
* Transition to a new LED state immediately.
|
|
||||||
* Resets animation timing for the new state.
|
|
||||||
*
|
|
||||||
* Arguments:
|
|
||||||
* - state: Target LED state (LedState enum)
|
|
||||||
*
|
|
||||||
* Returns: true if state changed, false if already in that state
|
|
||||||
*/
|
|
||||||
bool rgb_fsm_set_state(LedState state);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* rgb_fsm_get_state()
|
|
||||||
*
|
|
||||||
* Get current LED state.
|
|
||||||
*
|
|
||||||
* Returns: Current LED state (LedState enum)
|
|
||||||
*/
|
|
||||||
LedState rgb_fsm_get_state(void);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* rgb_fsm_tick(now_ms)
|
|
||||||
*
|
|
||||||
* Update function called periodically (recommended: every 10-50ms).
|
|
||||||
* Processes animations and timing for current state.
|
|
||||||
* Updates LED strip via PWM.
|
|
||||||
*
|
|
||||||
* Arguments:
|
|
||||||
* - now_ms: Current time in milliseconds (from HAL_GetTick() or similar)
|
|
||||||
*/
|
|
||||||
void rgb_fsm_tick(uint32_t now_ms);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* rgb_fsm_set_color(led_index, color)
|
|
||||||
*
|
|
||||||
* Set color of a specific LED (for testing and manual control).
|
|
||||||
* Bypasses current animation.
|
|
||||||
*
|
|
||||||
* Arguments:
|
|
||||||
* - led_index: 0-7 (LED ring has 8 LEDs)
|
|
||||||
* - color: RgbColor with R, G, B values (0-255)
|
|
||||||
*
|
|
||||||
* Returns: true if set, false if index out of range
|
|
||||||
*/
|
|
||||||
bool rgb_fsm_set_color(uint8_t led_index, RgbColor color);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* rgb_fsm_all_off()
|
|
||||||
*
|
|
||||||
* Turn off all LEDs immediately.
|
|
||||||
* Useful for shutdown or error conditions.
|
|
||||||
*/
|
|
||||||
void rgb_fsm_all_off(void);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* rgb_fsm_get_animation_frame()
|
|
||||||
*
|
|
||||||
* Get current animation progress (0-255).
|
|
||||||
* Useful for testing and debugging animation timing.
|
|
||||||
*
|
|
||||||
* Returns: Current frame value for animation (0-255 represents full cycle)
|
|
||||||
*/
|
|
||||||
uint8_t rgb_fsm_get_animation_frame(void);
|
|
||||||
|
|
||||||
#endif /* RGB_FSM_H */
|
|
||||||
332
src/rgb_fsm.c
332
src/rgb_fsm.c
@ -1,332 +0,0 @@
|
|||||||
#include "rgb_fsm.h"
|
|
||||||
#include "stm32f7xx_hal.h"
|
|
||||||
#include "config.h"
|
|
||||||
#include <string.h>
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
/* ================================================================
|
|
||||||
* WS2812 NeoPixel LED Strip Configuration
|
|
||||||
* ================================================================ */
|
|
||||||
|
|
||||||
#define NUM_LEDS 8
|
|
||||||
#define LED_STRIP_BITS (NUM_LEDS * 24) /* 24 bits per LED (RGB) */
|
|
||||||
|
|
||||||
/* ================================================================
|
|
||||||
* State Machine Internal State
|
|
||||||
* ================================================================ */
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
LedState current_state; /* Current operational state */
|
|
||||||
LedState previous_state; /* Previous state for transition detection */
|
|
||||||
uint32_t state_start_time_ms; /* When current state started */
|
|
||||||
uint32_t last_tick_ms; /* Last tick time */
|
|
||||||
uint8_t animation_frame; /* Current animation frame (0-255) */
|
|
||||||
RgbColor led_colors[NUM_LEDS]; /* Current color of each LED */
|
|
||||||
} RgbFsm;
|
|
||||||
|
|
||||||
static RgbFsm s_rgb = {
|
|
||||||
.current_state = LED_STATE_BOOT,
|
|
||||||
.previous_state = LED_STATE_BOOT,
|
|
||||||
.state_start_time_ms = 0,
|
|
||||||
.last_tick_ms = 0,
|
|
||||||
.animation_frame = 0
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ================================================================
|
|
||||||
* Color Definitions for Each State
|
|
||||||
* ================================================================ */
|
|
||||||
|
|
||||||
static const RgbColor COLOR_BLUE = { 0, 0, 255 };
|
|
||||||
static const RgbColor COLOR_GREEN = { 0, 255, 0 };
|
|
||||||
static const RgbColor COLOR_CYAN = { 0, 255, 255 };
|
|
||||||
static const RgbColor COLOR_RED = {255, 0, 0 };
|
|
||||||
static const RgbColor COLOR_ORANGE = {255, 165, 0 };
|
|
||||||
static const RgbColor COLOR_OFF = { 0, 0, 0 };
|
|
||||||
|
|
||||||
/* ================================================================
|
|
||||||
* Hardware Initialization
|
|
||||||
* ================================================================ */
|
|
||||||
|
|
||||||
void rgb_fsm_init(void)
|
|
||||||
{
|
|
||||||
/* Enable GPIO and timer clocks */
|
|
||||||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
|
||||||
__HAL_RCC_TIM3_CLK_ENABLE();
|
|
||||||
|
|
||||||
/* Configure PB4 as TIM3_CH1 PWM output */
|
|
||||||
GPIO_InitTypeDef gpio_init = {0};
|
|
||||||
gpio_init.Pin = LED_STRIP_PIN;
|
|
||||||
gpio_init.Mode = GPIO_MODE_AF_PP;
|
|
||||||
gpio_init.Pull = GPIO_NOPULL;
|
|
||||||
gpio_init.Speed = GPIO_SPEED_HIGH;
|
|
||||||
gpio_init.Alternate = LED_STRIP_AF;
|
|
||||||
HAL_GPIO_Init(LED_STRIP_PORT, &gpio_init);
|
|
||||||
|
|
||||||
/* Configure TIM3 for 800 kHz PWM
|
|
||||||
* Clock: 216MHz / PSC = output frequency
|
|
||||||
* For 800 kHz: PSC = 270, ARR = 100
|
|
||||||
* Duty cycle = CCR / ARR
|
|
||||||
*/
|
|
||||||
TIM_HandleTypeDef htim3 = {0};
|
|
||||||
htim3.Instance = LED_STRIP_TIM;
|
|
||||||
htim3.Init.Prescaler = 270 - 1; /* 216MHz / 270 = 800kHz clock */
|
|
||||||
htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
|
|
||||||
htim3.Init.Period = 100 - 1; /* 800kHz / 100 = 8 kHz PWM */
|
|
||||||
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
|
||||||
htim3.Init.RepetitionCounter = 0;
|
|
||||||
HAL_TIM_PWM_Init(&htim3);
|
|
||||||
|
|
||||||
/* Configure PWM on CH1 for WS2812 signal */
|
|
||||||
TIM_OC_InitTypeDef oc_init = {0};
|
|
||||||
oc_init.OCMode = TIM_OCMODE_PWM1;
|
|
||||||
oc_init.Pulse = 0; /* Start at 0% duty (off) */
|
|
||||||
oc_init.OCPolarity = TIM_OCPOLARITY_HIGH;
|
|
||||||
oc_init.OCFastMode = TIM_OCFAST_DISABLE;
|
|
||||||
HAL_TIM_PWM_ConfigChannel(&htim3, &oc_init, LED_STRIP_CHANNEL);
|
|
||||||
|
|
||||||
/* Start PWM generation */
|
|
||||||
HAL_TIM_PWM_Start(LED_STRIP_TIM, LED_STRIP_CHANNEL);
|
|
||||||
|
|
||||||
/* Initialize state */
|
|
||||||
s_rgb.current_state = LED_STATE_BOOT;
|
|
||||||
s_rgb.previous_state = LED_STATE_BOOT;
|
|
||||||
s_rgb.state_start_time_ms = 0;
|
|
||||||
s_rgb.animation_frame = 0;
|
|
||||||
memset(s_rgb.led_colors, 0, sizeof(s_rgb.led_colors));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ================================================================
|
|
||||||
* LED Update Function
|
|
||||||
* ================================================================ */
|
|
||||||
|
|
||||||
static void rgb_update_led_strip(void)
|
|
||||||
{
|
|
||||||
/* Calculate total brightness from LED colors */
|
|
||||||
uint32_t total_brightness = 0;
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
total_brightness += s_rgb.led_colors[i].r;
|
|
||||||
total_brightness += s_rgb.led_colors[i].g;
|
|
||||||
total_brightness += s_rgb.led_colors[i].b;
|
|
||||||
}
|
|
||||||
/* Normalize to 0-100 (PWM duty cycle) */
|
|
||||||
uint32_t duty = (total_brightness / (NUM_LEDS * 3)) * 100 / 255;
|
|
||||||
if (duty > 100) duty = 100;
|
|
||||||
|
|
||||||
TIM3->CCR1 = (duty * 100) / 100; /* Set duty cycle */
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ================================================================
|
|
||||||
* Animation Functions
|
|
||||||
* ================================================================ */
|
|
||||||
|
|
||||||
static void animate_boot(uint32_t elapsed_ms)
|
|
||||||
{
|
|
||||||
/* Blue pulse at 0.5 Hz (2 second period) */
|
|
||||||
uint32_t period_ms = 2000;
|
|
||||||
uint8_t phase = (elapsed_ms % period_ms) * 255 / period_ms;
|
|
||||||
uint8_t brightness = (uint8_t)(128 + 127 * sin(2 * 3.14159 * phase / 256));
|
|
||||||
|
|
||||||
RgbColor color = COLOR_BLUE;
|
|
||||||
color.b = (color.b * brightness) / 255;
|
|
||||||
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
s_rgb.led_colors[i] = color;
|
|
||||||
}
|
|
||||||
s_rgb.animation_frame = phase;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void animate_idle(uint32_t elapsed_ms)
|
|
||||||
{
|
|
||||||
/* Green breathe at 0.5 Hz (2 second period) */
|
|
||||||
uint32_t period_ms = 2000;
|
|
||||||
uint8_t phase = (elapsed_ms % period_ms) * 255 / period_ms;
|
|
||||||
uint8_t brightness = (uint8_t)(128 + 127 * sin(2 * 3.14159 * phase / 256));
|
|
||||||
|
|
||||||
RgbColor color = COLOR_GREEN;
|
|
||||||
color.g = (color.g * brightness) / 255;
|
|
||||||
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
s_rgb.led_colors[i] = color;
|
|
||||||
}
|
|
||||||
s_rgb.animation_frame = phase;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void animate_armed(uint32_t elapsed_ms)
|
|
||||||
{
|
|
||||||
/* Solid green (no animation) */
|
|
||||||
(void)elapsed_ms;
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
s_rgb.led_colors[i] = COLOR_GREEN;
|
|
||||||
}
|
|
||||||
s_rgb.animation_frame = 255;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void animate_nav(uint32_t elapsed_ms)
|
|
||||||
{
|
|
||||||
/* Cyan spin (rotating pattern at 1 Hz) */
|
|
||||||
uint32_t period_ms = 1000;
|
|
||||||
uint8_t phase = (elapsed_ms % period_ms) * 8 / period_ms;
|
|
||||||
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
if (i == phase) {
|
|
||||||
s_rgb.led_colors[i] = COLOR_CYAN;
|
|
||||||
} else if (i == (phase + 7) % 8) {
|
|
||||||
s_rgb.led_colors[i] = (RgbColor){0, 128, 128};
|
|
||||||
} else {
|
|
||||||
s_rgb.led_colors[i] = COLOR_OFF;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
s_rgb.animation_frame = (uint8_t)phase * 32;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void animate_error(uint32_t elapsed_ms)
|
|
||||||
{
|
|
||||||
/* Red flash at 2 Hz (500ms period) */
|
|
||||||
uint32_t period_ms = 500;
|
|
||||||
uint8_t brightness = ((elapsed_ms % period_ms) < 250) ? 255 : 0;
|
|
||||||
|
|
||||||
RgbColor color = COLOR_RED;
|
|
||||||
color.r = (color.r * brightness) / 255;
|
|
||||||
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
s_rgb.led_colors[i] = color;
|
|
||||||
}
|
|
||||||
s_rgb.animation_frame = brightness;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void animate_low_batt(uint32_t elapsed_ms)
|
|
||||||
{
|
|
||||||
/* Orange blink at 1 Hz (1000ms period) */
|
|
||||||
uint32_t period_ms = 1000;
|
|
||||||
uint8_t brightness = ((elapsed_ms % period_ms) < 500) ? 255 : 0;
|
|
||||||
|
|
||||||
RgbColor color = COLOR_ORANGE;
|
|
||||||
color.r = (color.r * brightness) / 255;
|
|
||||||
color.g = (color.g * brightness) / 255;
|
|
||||||
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
s_rgb.led_colors[i] = color;
|
|
||||||
}
|
|
||||||
s_rgb.animation_frame = brightness;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void animate_charging(uint32_t elapsed_ms)
|
|
||||||
{
|
|
||||||
/* Green fill (progressive LEDs lighting up at 1 Hz) */
|
|
||||||
uint32_t period_ms = 1000;
|
|
||||||
uint8_t phase = (elapsed_ms % period_ms) * 8 / period_ms;
|
|
||||||
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
if (i < phase) {
|
|
||||||
s_rgb.led_colors[i] = COLOR_GREEN;
|
|
||||||
} else {
|
|
||||||
s_rgb.led_colors[i] = COLOR_OFF;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
s_rgb.animation_frame = phase * 32;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void animate_estop(uint32_t elapsed_ms)
|
|
||||||
{
|
|
||||||
/* Red solid (full intensity, no animation) */
|
|
||||||
(void)elapsed_ms;
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
s_rgb.led_colors[i] = COLOR_RED;
|
|
||||||
}
|
|
||||||
s_rgb.animation_frame = 255;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ================================================================
|
|
||||||
* Public API
|
|
||||||
* ================================================================ */
|
|
||||||
|
|
||||||
bool rgb_fsm_set_state(LedState state)
|
|
||||||
{
|
|
||||||
if (state >= LED_STATE_COUNT) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (state == s_rgb.current_state) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
s_rgb.previous_state = s_rgb.current_state;
|
|
||||||
s_rgb.current_state = state;
|
|
||||||
s_rgb.state_start_time_ms = 0;
|
|
||||||
s_rgb.animation_frame = 0;
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
LedState rgb_fsm_get_state(void)
|
|
||||||
{
|
|
||||||
return s_rgb.current_state;
|
|
||||||
}
|
|
||||||
|
|
||||||
void rgb_fsm_tick(uint32_t now_ms)
|
|
||||||
{
|
|
||||||
if (s_rgb.state_start_time_ms == 0) {
|
|
||||||
s_rgb.state_start_time_ms = now_ms;
|
|
||||||
s_rgb.last_tick_ms = now_ms;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t elapsed = now_ms - s_rgb.state_start_time_ms;
|
|
||||||
|
|
||||||
switch (s_rgb.current_state) {
|
|
||||||
case LED_STATE_BOOT:
|
|
||||||
animate_boot(elapsed);
|
|
||||||
break;
|
|
||||||
case LED_STATE_IDLE:
|
|
||||||
animate_idle(elapsed);
|
|
||||||
break;
|
|
||||||
case LED_STATE_ARMED:
|
|
||||||
animate_armed(elapsed);
|
|
||||||
break;
|
|
||||||
case LED_STATE_NAV:
|
|
||||||
animate_nav(elapsed);
|
|
||||||
break;
|
|
||||||
case LED_STATE_ERROR:
|
|
||||||
animate_error(elapsed);
|
|
||||||
break;
|
|
||||||
case LED_STATE_LOW_BATT:
|
|
||||||
animate_low_batt(elapsed);
|
|
||||||
break;
|
|
||||||
case LED_STATE_CHARGING:
|
|
||||||
animate_charging(elapsed);
|
|
||||||
break;
|
|
||||||
case LED_STATE_ESTOP:
|
|
||||||
animate_estop(elapsed);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
rgb_fsm_all_off();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
rgb_update_led_strip();
|
|
||||||
s_rgb.last_tick_ms = now_ms;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool rgb_fsm_set_color(uint8_t led_index, RgbColor color)
|
|
||||||
{
|
|
||||||
if (led_index >= NUM_LEDS) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
s_rgb.led_colors[led_index] = color;
|
|
||||||
rgb_update_led_strip();
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void rgb_fsm_all_off(void)
|
|
||||||
{
|
|
||||||
for (int i = 0; i < NUM_LEDS; i++) {
|
|
||||||
s_rgb.led_colors[i] = COLOR_OFF;
|
|
||||||
}
|
|
||||||
rgb_update_led_strip();
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t rgb_fsm_get_animation_frame(void)
|
|
||||||
{
|
|
||||||
return s_rgb.animation_frame;
|
|
||||||
}
|
|
||||||
@ -1,265 +0,0 @@
|
|||||||
/*
|
|
||||||
* test_rgb_fsm.c — RGB Status LED State Machine tests (Issue #290)
|
|
||||||
*
|
|
||||||
* Verifies:
|
|
||||||
* - State transitions and initial state
|
|
||||||
* - Animation progression for each LED state
|
|
||||||
* - Timing and animation cycles
|
|
||||||
* - State-specific animations (pulse, breathe, spin, blink, fill)
|
|
||||||
* - Edge cases and invalid inputs
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
/* ── LED State Machine Simulator ──────────────────────────────────*/
|
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
LED_STATE_BOOT = 0,
|
|
||||||
LED_STATE_IDLE,
|
|
||||||
LED_STATE_ARMED,
|
|
||||||
LED_STATE_NAV,
|
|
||||||
LED_STATE_ERROR,
|
|
||||||
LED_STATE_LOW_BATT,
|
|
||||||
LED_STATE_CHARGING,
|
|
||||||
LED_STATE_ESTOP,
|
|
||||||
LED_STATE_COUNT
|
|
||||||
} LedState;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
LedState current_state;
|
|
||||||
LedState previous_state;
|
|
||||||
uint32_t state_start_time_ms;
|
|
||||||
uint32_t last_tick_ms;
|
|
||||||
uint8_t animation_frame;
|
|
||||||
} RgbFsm;
|
|
||||||
|
|
||||||
static RgbFsm sim = {0};
|
|
||||||
|
|
||||||
void sim_init(void) {
|
|
||||||
memset(&sim, 0, sizeof(sim));
|
|
||||||
sim.current_state = LED_STATE_BOOT;
|
|
||||||
sim.previous_state = LED_STATE_BOOT;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool sim_set_state(LedState state) {
|
|
||||||
if (state >= LED_STATE_COUNT) return false;
|
|
||||||
if (state == sim.current_state) return false;
|
|
||||||
sim.previous_state = sim.current_state;
|
|
||||||
sim.current_state = state;
|
|
||||||
sim.state_start_time_ms = (uint32_t)-1;
|
|
||||||
sim.animation_frame = 0;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
LedState sim_get_state(void) {
|
|
||||||
return sim.current_state;
|
|
||||||
}
|
|
||||||
|
|
||||||
void sim_tick(uint32_t now_ms) {
|
|
||||||
if (sim.state_start_time_ms == (uint32_t)-1) {
|
|
||||||
sim.state_start_time_ms = now_ms;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t elapsed = now_ms - sim.state_start_time_ms;
|
|
||||||
|
|
||||||
switch (sim.current_state) {
|
|
||||||
case LED_STATE_BOOT:
|
|
||||||
sim.animation_frame = (elapsed % 2000) * 255 / 2000;
|
|
||||||
break;
|
|
||||||
case LED_STATE_IDLE:
|
|
||||||
sim.animation_frame = (elapsed % 2000) * 255 / 2000;
|
|
||||||
break;
|
|
||||||
case LED_STATE_ARMED:
|
|
||||||
sim.animation_frame = 255;
|
|
||||||
break;
|
|
||||||
case LED_STATE_NAV:
|
|
||||||
sim.animation_frame = ((elapsed % 1000) / 125) * 32;
|
|
||||||
break;
|
|
||||||
case LED_STATE_ERROR:
|
|
||||||
sim.animation_frame = ((elapsed % 500) < 250) ? 255 : 0;
|
|
||||||
break;
|
|
||||||
case LED_STATE_LOW_BATT:
|
|
||||||
sim.animation_frame = ((elapsed % 1000) < 500) ? 255 : 0;
|
|
||||||
break;
|
|
||||||
case LED_STATE_CHARGING:
|
|
||||||
sim.animation_frame = ((elapsed % 1000) / 125) * 32;
|
|
||||||
break;
|
|
||||||
case LED_STATE_ESTOP:
|
|
||||||
sim.animation_frame = 255;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
sim.last_tick_ms = now_ms;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ── Unit Tests ────────────────────────────────────────────────────────*/
|
|
||||||
|
|
||||||
static int test_count = 0, test_passed = 0, test_failed = 0;
|
|
||||||
|
|
||||||
#define TEST(name) do { test_count++; printf("\n TEST %d: %s\n", test_count, name); } while(0)
|
|
||||||
#define ASSERT(cond, msg) do { if (cond) { test_passed++; printf(" ✓ %s\n", msg); } else { test_failed++; printf(" ✗ %s\n", msg); } } while(0)
|
|
||||||
|
|
||||||
void test_initial_state(void) {
|
|
||||||
TEST("Initial state is BOOT");
|
|
||||||
sim_init();
|
|
||||||
ASSERT(sim_get_state() == LED_STATE_BOOT, "State is BOOT");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_state_transitions(void) {
|
|
||||||
TEST("State transitions work correctly");
|
|
||||||
sim_init();
|
|
||||||
bool result = sim_set_state(LED_STATE_IDLE);
|
|
||||||
ASSERT(result == true, "Transition succeeds");
|
|
||||||
ASSERT(sim_get_state() == LED_STATE_IDLE, "State changed");
|
|
||||||
result = sim_set_state(LED_STATE_IDLE);
|
|
||||||
ASSERT(result == false, "Same state returns false");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_all_states(void) {
|
|
||||||
TEST("All 8 states are accessible");
|
|
||||||
sim_init();
|
|
||||||
for (int i = 1; i < LED_STATE_COUNT; i++) {
|
|
||||||
bool result = sim_set_state((LedState)i);
|
|
||||||
ASSERT(result == true, "State transition succeeds");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_boot_animation(void) {
|
|
||||||
TEST("BOOT state animates");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_BOOT);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(500);
|
|
||||||
uint8_t frame = sim.animation_frame;
|
|
||||||
ASSERT(frame > 0 && frame < 255, "Animation progresses");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_idle_animation(void) {
|
|
||||||
TEST("IDLE state animates");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_IDLE);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(500);
|
|
||||||
ASSERT(sim.animation_frame > 0, "Animation starts");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_armed_static(void) {
|
|
||||||
TEST("ARMED state is static");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_ARMED);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(100);
|
|
||||||
ASSERT(sim.animation_frame == 255, "No animation");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_nav_animation(void) {
|
|
||||||
TEST("NAV state spins");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_NAV);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(150);
|
|
||||||
ASSERT(sim.animation_frame > 0, "Animation starts");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_error_animation(void) {
|
|
||||||
TEST("ERROR state flashes");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_ERROR);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(100);
|
|
||||||
ASSERT(sim.animation_frame == 255, "Bright state");
|
|
||||||
sim_tick(300);
|
|
||||||
ASSERT(sim.animation_frame == 0, "Dark state");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_low_batt_animation(void) {
|
|
||||||
TEST("LOW_BATT state blinks");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_LOW_BATT);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(100);
|
|
||||||
ASSERT(sim.animation_frame == 255, "Bright");
|
|
||||||
sim_tick(600);
|
|
||||||
ASSERT(sim.animation_frame == 0, "Dark");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_charging_animation(void) {
|
|
||||||
TEST("CHARGING state fills");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_CHARGING);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(200);
|
|
||||||
ASSERT(sim.animation_frame > 0, "Animation progresses");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_estop_static(void) {
|
|
||||||
TEST("ESTOP state is static");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_ESTOP);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(100);
|
|
||||||
ASSERT(sim.animation_frame == 255, "Solid red");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_state_reset(void) {
|
|
||||||
TEST("State change resets timing");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_BOOT);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(500);
|
|
||||||
uint8_t boot_frame = sim.animation_frame;
|
|
||||||
sim_set_state(LED_STATE_IDLE);
|
|
||||||
sim_tick(0);
|
|
||||||
sim_tick(100);
|
|
||||||
uint8_t idle_frame = sim.animation_frame;
|
|
||||||
ASSERT(idle_frame < boot_frame, "Fresh animation start");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_invalid_state(void) {
|
|
||||||
TEST("Invalid state rejected");
|
|
||||||
sim_init();
|
|
||||||
bool result = sim_set_state((LedState)255);
|
|
||||||
ASSERT(result == false, "Invalid rejected");
|
|
||||||
}
|
|
||||||
|
|
||||||
void test_rapid_changes(void) {
|
|
||||||
TEST("Rapid transitions work");
|
|
||||||
sim_init();
|
|
||||||
sim_set_state(LED_STATE_IDLE);
|
|
||||||
ASSERT(sim_get_state() == LED_STATE_IDLE, "In IDLE");
|
|
||||||
sim_set_state(LED_STATE_ERROR);
|
|
||||||
ASSERT(sim_get_state() == LED_STATE_ERROR, "In ERROR");
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(void) {
|
|
||||||
printf("\n══════════════════════════════════════════════════════════════\n");
|
|
||||||
printf(" RGB Status LED State Machine — Unit Tests (Issue #290)\n");
|
|
||||||
printf("══════════════════════════════════════════════════════════════\n");
|
|
||||||
|
|
||||||
test_initial_state();
|
|
||||||
test_state_transitions();
|
|
||||||
test_all_states();
|
|
||||||
test_boot_animation();
|
|
||||||
test_idle_animation();
|
|
||||||
test_armed_static();
|
|
||||||
test_nav_animation();
|
|
||||||
test_error_animation();
|
|
||||||
test_low_batt_animation();
|
|
||||||
test_charging_animation();
|
|
||||||
test_estop_static();
|
|
||||||
test_state_reset();
|
|
||||||
test_invalid_state();
|
|
||||||
test_rapid_changes();
|
|
||||||
|
|
||||||
printf("\n──────────────────────────────────────────────────────────────\n");
|
|
||||||
printf(" Results: %d/%d tests passed, %d failed\n", test_passed, test_count, test_failed);
|
|
||||||
printf("──────────────────────────────────────────────────────────────\n\n");
|
|
||||||
|
|
||||||
return (test_failed == 0) ? 0 : 1;
|
|
||||||
}
|
|
||||||
Loading…
x
Reference in New Issue
Block a user