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

Merge pull request 'feat: Add RGB status LED state machine (Issue #290)' (#294) from sl-firmware/issue-290-rgb-fsm into main

Browse Source
This commit is contained in:
sl-jetson 2026-03-02 21:05:34 -05:00
commit b3edabc9c5
3 changed files with 721 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
include/rgb_fsm.h Normal file
View File

@ -0,0 +1,124 @@
#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 Normal file
View File

@ -0,0 +1,332 @@
#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;
}

265
test/test_rgb_fsm.c Normal file
View File

@ -0,0 +1,265 @@
/*
* 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;
}