From 9d84153febd057d95eb107bdc7512e76ddbf6d8f Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 16 Sep 2025 12:35:17 +0200 Subject: [PATCH] Added status heartbeat effect for power on and idle --- rp2040/src/hardware.rs | 4 +- rp2040/src/main.rs | 7 +- rp2040/src/status.rs | 371 +++++++++++++++++++++++++++++++---------- 3 files changed, 294 insertions(+), 88 deletions(-) diff --git a/rp2040/src/hardware.rs b/rp2040/src/hardware.rs index 713a440..6451938 100644 --- a/rp2040/src/hardware.rs +++ b/rp2040/src/hardware.rs @@ -85,7 +85,7 @@ pub mod i2c { /// Cadences for periodic firmware tasks. pub mod timers { /// Status LED update interval (ms). - pub const STATUS_LED_INTERVAL_MS: u32 = 250; + pub const STATUS_LED_INTERVAL_MS: u32 = 40; /// Button matrix scan interval (µs). pub const SCAN_INTERVAL_US: u32 = 200; @@ -97,7 +97,7 @@ pub mod timers { pub const USB_UPDATE_INTERVAL_MS: u32 = 10; /// USB activity timeout (ms) - stop sending reports after this period of inactivity. - pub const USB_ACTIVITY_TIMEOUT_MS: u32 = 10_000; // 10 seconds + pub const USB_ACTIVITY_TIMEOUT_MS: u32 = 5_000; // 5 seconds } // ==================== USB DEVICE CONFIGURATION ==================== diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index b523ac6..4d81399 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -232,6 +232,7 @@ fn main() -> ! { let mut usb_activity: bool = false; let mut usb_active: bool = false; + let mut usb_initialized: bool = false; let mut vt_enable: bool = false; let mut idle_mode: bool = false; let mut usb_activity_timeout_count: u32 = 0; @@ -287,6 +288,9 @@ fn main() -> ! { // Handle USB device polling and maintain connection state if usb_dev.poll(&mut [&mut usb_hid_joystick]) { + if !usb_initialized { + usb_initialized = true; + } if !usb_active { usb_activity = true; // Force initial report idle_mode = false; @@ -336,6 +340,7 @@ fn main() -> ! { let system_state = SystemState { usb_active, + usb_initialized, idle_mode, calibration_active: calibration_manager.is_active(), throttle_hold_enable: axis_manager.throttle_hold_enable, @@ -494,8 +499,6 @@ fn main() -> ! { } } else if usb_tick && usb_active { idle_mode = true; - } else if usb_tick { - idle_mode = false; } } } diff --git a/rp2040/src/status.rs b/rp2040/src/status.rs index ae81bb3..d9e675d 100644 --- a/rp2040/src/status.rs +++ b/rp2040/src/status.rs @@ -11,6 +11,13 @@ use rp2040_hal::{ use smart_leds::{SmartLedsWrite, RGB8}; use ws2812_pio::Ws2812Direct; +const COLOR_OFF: RGB8 = RGB8 { r: 0, g: 0, b: 0 }; +const COLOR_GREEN: RGB8 = RGB8 { r: 10, g: 7, b: 0 }; +const COLOR_BLUE: RGB8 = RGB8 { r: 10, g: 4, b: 10 }; +const COLOR_ORANGE: RGB8 = RGB8 { r: 5, g: 10, b: 0 }; +const COLOR_RED: RGB8 = RGB8 { r: 20, g: 0, b: 0 }; +const COLOR_PURPLE: RGB8 = RGB8 { r: 0, g: 10, b: 10 }; + /// Status LED modes with clear semantics. #[allow(dead_code)] #[derive(PartialEq, Eq, Copy, Clone, Debug)] @@ -26,48 +33,185 @@ pub enum StatusMode { Warning = 8, Error = 9, Bootloader = 10, + Power = 11, } /// Aggregate system state for LED status indication. #[derive(Clone, Copy)] pub struct SystemState { pub usb_active: bool, + pub usb_initialized: bool, pub idle_mode: bool, pub calibration_active: bool, pub throttle_hold_enable: bool, pub vt_enable: bool, } -/// Color definitions for different status modes. -const LED_COLORS: [RGB8; 11] = [ - RGB8 { r: 0, g: 0, b: 0 }, // Off - RGB8 { r: 10, g: 7, b: 0 }, // Normal (Green) - RGB8 { r: 10, g: 7, b: 0 }, // NormalFlash (Green) - RGB8 { r: 10, g: 4, b: 10 }, // Activity (Blue) - RGB8 { r: 10, g: 4, b: 10 }, // ActivityFlash (Blue) - RGB8 { r: 10, g: 7, b: 0 }, // Idle (Green flash) - RGB8 { r: 5, g: 10, b: 0 }, // Other (Orange) - RGB8 { r: 5, g: 10, b: 0 }, // OtherFlash (Orange) - RGB8 { r: 2, g: 20, b: 0 }, // Warning (Red) - RGB8 { r: 2, g: 20, b: 0 }, // Error (Red) - RGB8 { r: 0, g: 10, b: 10 }, // Bootloader (Purple) -]; +#[derive(Copy, Clone)] +enum LedEffect { + Solid, + Blink { period_ms: u32 }, + Heartbeat { period_ms: u32 }, +} -fn determine_mode(system_state: SystemState) -> StatusMode { +#[derive(Copy, Clone)] +struct ModeDescriptor { + color: RGB8, + effect: LedEffect, +} + +const HEARTBEAT_POWER_MS: u32 = 800; +const HEARTBEAT_IDLE_MS: u32 = 3200; + +impl LedEffect { + fn update_interval_ms(self) -> u32 { + match self { + LedEffect::Solid => 0, + LedEffect::Blink { period_ms } => period_ms / 2, + LedEffect::Heartbeat { .. } => 40, + } + } + + fn color_for(self, base: RGB8, elapsed_ms: u32) -> RGB8 { + match self { + LedEffect::Solid => base, + LedEffect::Blink { period_ms } => { + if period_ms == 0 { + return base; + } + let half = (period_ms / 2).max(1); + if elapsed_ms % period_ms < half { + base + } else { + RGB8 { r: 0, g: 0, b: 0 } + } + } + LedEffect::Heartbeat { period_ms } => { + let period = period_ms.max(1); + let phase = elapsed_ms % period; + let half = (period / 2).max(1); + let ramp = if phase < half { + ((phase * 255) / half) as u8 + } else { + (((period - phase) * 255) / half) as u8 + }; + scale_color(base, ramp) + } + } + } +} + +const fn descriptor_for(mode: StatusMode, base_mode: StatusMode) -> ModeDescriptor { + match mode { + StatusMode::Off => ModeDescriptor { + color: COLOR_OFF, + effect: LedEffect::Solid, + }, + StatusMode::Normal => ModeDescriptor { + color: COLOR_GREEN, + effect: LedEffect::Solid, + }, + StatusMode::NormalFlash => ModeDescriptor { + color: COLOR_GREEN, + effect: LedEffect::Blink { period_ms: 1000 }, + }, + StatusMode::Activity => ModeDescriptor { + color: COLOR_BLUE, + effect: LedEffect::Solid, + }, + StatusMode::ActivityFlash => ModeDescriptor { + color: COLOR_BLUE, + effect: LedEffect::Blink { period_ms: 600 }, + }, + StatusMode::Idle => match base_mode { + StatusMode::Activity | StatusMode::ActivityFlash => ModeDescriptor { + color: COLOR_BLUE, + effect: LedEffect::Heartbeat { + period_ms: HEARTBEAT_IDLE_MS, + }, + }, + StatusMode::Other | StatusMode::OtherFlash => ModeDescriptor { + color: COLOR_ORANGE, + effect: LedEffect::Heartbeat { + period_ms: HEARTBEAT_IDLE_MS, + }, + }, + StatusMode::NormalFlash | StatusMode::Normal => ModeDescriptor { + color: COLOR_GREEN, + effect: LedEffect::Heartbeat { + period_ms: HEARTBEAT_IDLE_MS, + }, + }, + StatusMode::Warning => ModeDescriptor { + color: COLOR_RED, + effect: LedEffect::Heartbeat { + period_ms: HEARTBEAT_IDLE_MS, + }, + }, + StatusMode::Error => ModeDescriptor { + color: COLOR_RED, + effect: LedEffect::Heartbeat { + period_ms: HEARTBEAT_IDLE_MS, + }, + }, + _ => ModeDescriptor { + color: COLOR_GREEN, + effect: LedEffect::Heartbeat { + period_ms: HEARTBEAT_IDLE_MS, + }, + }, + }, + StatusMode::Other => ModeDescriptor { + color: COLOR_ORANGE, + effect: LedEffect::Solid, + }, + StatusMode::OtherFlash => ModeDescriptor { + color: COLOR_ORANGE, + effect: LedEffect::Blink { period_ms: 600 }, + }, + StatusMode::Warning => ModeDescriptor { + color: COLOR_RED, + effect: LedEffect::Blink { period_ms: 500 }, + }, + StatusMode::Error => ModeDescriptor { + color: COLOR_RED, + effect: LedEffect::Solid, + }, + StatusMode::Bootloader => ModeDescriptor { + color: COLOR_PURPLE, + effect: LedEffect::Solid, + }, + StatusMode::Power => ModeDescriptor { + color: COLOR_GREEN, + effect: LedEffect::Heartbeat { + period_ms: HEARTBEAT_POWER_MS, + }, + }, + } +} + +fn scale_color(base: RGB8, brightness: u8) -> RGB8 { + let scale = brightness as u16; + RGB8 { + r: ((base.r as u16 * scale) / 255) as u8, + g: ((base.g as u16 * scale) / 255) as u8, + b: ((base.b as u16 * scale) / 255) as u8, + } +} + +fn determine_base_mode(system_state: SystemState) -> StatusMode { if system_state.calibration_active { StatusMode::ActivityFlash - } else if system_state.idle_mode { - StatusMode::Idle + } else if !system_state.usb_initialized { + StatusMode::Power } else if !system_state.usb_active { StatusMode::NormalFlash } else if system_state.vt_enable { StatusMode::Activity } else if system_state.throttle_hold_enable { StatusMode::Other - } else if system_state.usb_active { - StatusMode::Normal } else { - StatusMode::Off + StatusMode::Normal } } @@ -80,7 +224,7 @@ where { ws2812_direct: Ws2812Direct, current_mode: StatusMode, - flash_state: bool, + mode_started_at: Option, last_update_time: Option, } @@ -104,85 +248,77 @@ where clock_freq: fugit::HertzU32, ) -> Self { let ws2812_direct = Ws2812Direct::new(pin, pio, sm, clock_freq); - Self { + let mut status = Self { ws2812_direct, current_mode: StatusMode::Off, - flash_state: false, + mode_started_at: None, last_update_time: None, - } + }; + status.write_color(COLOR_OFF); + status } /// Update LED based on system state and current time (ms). pub fn update_from_system_state(&mut self, system_state: SystemState, current_time: u32) { - let desired_mode = determine_mode(system_state); - self.set_mode(desired_mode, current_time); + let base_mode = determine_base_mode(system_state); + let idle_overlay = system_state.idle_mode + && system_state.usb_initialized + && base_mode != StatusMode::Off + && base_mode != StatusMode::Power; + let desired_mode = if idle_overlay { + (StatusMode::Idle, base_mode) + } else { + (base_mode, base_mode) + }; + + self.set_mode(desired_mode.0, desired_mode.1, current_time); } /// Set LED mode directly (explicit override). - pub fn set_mode(&mut self, mode: StatusMode, current_time: u32) { + pub fn set_mode(&mut self, mode: StatusMode, base_mode: StatusMode, current_time: u32) { // Force update if mode changed let force_update = mode != self.current_mode; self.current_mode = mode; + let descriptor = descriptor_for(mode, base_mode); + if force_update { + let start_time = match descriptor.effect { + LedEffect::Heartbeat { period_ms } => { + let half = (period_ms / 2).max(1); + current_time.saturating_sub(half) + } + _ => current_time, + }; + self.mode_started_at = Some(start_time); + self.last_update_time = None; + } - self.update_display(current_time, force_update); + self.update_display(current_time, force_update, base_mode); } /// Periodic update for flashing behavior at roughly ~500 ms intervals. - pub fn update_display(&mut self, current_time: u32, force_update: bool) { - let should_update = force_update || self.should_flash_now(current_time); + pub fn update_display(&mut self, current_time: u32, force_update: bool, base_mode: StatusMode) { + let descriptor = descriptor_for(self.current_mode, base_mode); + let interval = descriptor.effect.update_interval_ms(); - if !should_update { - return; + if !force_update { + if interval == 0 { + return; + } + if let Some(last) = self.last_update_time { + if current_time.saturating_sub(last) < interval { + return; + } + } } + let elapsed = self + .mode_started_at + .map(|start| current_time.saturating_sub(start)) + .unwrap_or(0); + let color = descriptor.effect.color_for(descriptor.color, elapsed); + + self.write_color(color); self.last_update_time = Some(current_time); - - match self.current_mode { - // Flashing modes - toggle between on and off - StatusMode::NormalFlash - | StatusMode::ActivityFlash - | StatusMode::Idle - | StatusMode::OtherFlash - | StatusMode::Warning => { - if self.flash_state { - // Show the color - self.write_color(LED_COLORS[self.current_mode as usize]); - } else { - // Show off (black) - self.write_color(LED_COLORS[StatusMode::Off as usize]); - } - self.flash_state = !self.flash_state; - } - // Solid modes - just show the color - _ => { - self.write_color(LED_COLORS[self.current_mode as usize]); - self.flash_state = true; // Reset flash state for next flash mode - } - } - } - - /// Get current status mode. - #[allow(dead_code)] - pub fn get_mode(&self) -> StatusMode { - self.current_mode - } - - /// Check if it's time to update the flashing LED state. - fn should_flash_now(&self, current_time: u32) -> bool { - match self.last_update_time { - None => true, // First update - Some(last_time) => { - // Flash every ~500ms for flashing modes - match self.current_mode { - StatusMode::NormalFlash - | StatusMode::ActivityFlash - | StatusMode::Idle - | StatusMode::OtherFlash - | StatusMode::Warning => current_time.saturating_sub(last_time) >= 500, - _ => false, // Non-flashing modes don't need periodic updates - } - } - } } /// Write a single color to the LED. @@ -200,7 +336,7 @@ where /// Legacy interface for compatibility – direct mode update. pub fn update(&mut self, mode: StatusMode) { // Use a dummy time for immediate updates - self.set_mode(mode, 0); + self.set_mode(mode, mode, 0); } } @@ -209,28 +345,95 @@ mod tests { use super::*; #[test] - fn idle_mode_selected_when_usb_idle() { + fn idle_mode_uses_base_color_with_heartbeat() { let state = SystemState { - usb_active: true, + usb_active: false, + usb_initialized: true, idle_mode: true, calibration_active: false, throttle_hold_enable: false, vt_enable: false, }; - assert_eq!(determine_mode(state), StatusMode::Idle); + let base = determine_base_mode(state); + assert_eq!(base, StatusMode::NormalFlash); + + let descriptor = descriptor_for(StatusMode::Idle, base); + let LedEffect::Heartbeat { .. } = descriptor.effect else { + panic!("Idle should always use heartbeat effect"); + }; + assert_eq!(descriptor.color, COLOR_GREEN); + } + + #[test] + fn power_mode_uses_fast_heartbeat() { + let descriptor = descriptor_for(StatusMode::Power, StatusMode::Normal); + if let LedEffect::Heartbeat { period_ms } = descriptor.effect { + assert_eq!(period_ms, HEARTBEAT_POWER_MS); + assert_eq!(descriptor.color, COLOR_GREEN); + } else { + panic!("Power mode must use heartbeat effect"); + } } #[test] fn calibration_has_priority_over_idle() { let state = SystemState { usb_active: true, + usb_initialized: true, idle_mode: true, calibration_active: true, throttle_hold_enable: false, vt_enable: false, }; - assert_eq!(determine_mode(state), StatusMode::ActivityFlash); + let base = determine_base_mode(state); + assert_eq!(base, StatusMode::ActivityFlash); + } + + #[test] + fn heartbeat_effect_fades() { + let base = StatusMode::Normal; + let descriptor = descriptor_for(StatusMode::Idle, base); + let LedEffect::Heartbeat { period_ms } = descriptor.effect else { + panic!("Idle should use heartbeat effect"); + }; + + let start = descriptor.effect.color_for(descriptor.color, 0); + let quarter = descriptor.effect.color_for(descriptor.color, period_ms / 4); + let half = descriptor.effect.color_for(descriptor.color, period_ms / 2); + let end = descriptor.effect.color_for(descriptor.color, period_ms); + + assert_eq!(start.g, 0); + assert!(quarter.g > start.g); + assert_eq!(half, descriptor.color); + assert_eq!(end.g, 0); + } + + #[test] + fn blink_effect_toggles() { + let descriptor = descriptor_for(StatusMode::NormalFlash, StatusMode::NormalFlash); + let LedEffect::Blink { period_ms } = descriptor.effect else { + panic!("NormalFlash should use blink effect"); + }; + + let on = descriptor.effect.color_for(descriptor.color, 0); + let off = descriptor.effect.color_for(descriptor.color, period_ms / 2); + assert_eq!(on, descriptor.color); + assert_eq!(off, COLOR_OFF); + } + + #[test] + fn determine_base_mode_before_usb() { + let state = SystemState { + usb_active: false, + usb_initialized: false, + idle_mode: false, + calibration_active: false, + throttle_hold_enable: false, + vt_enable: false, + }; + + assert_eq!(determine_base_mode(state), StatusMode::Power); } }