Added status heartbeat effect for power on and idle

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

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

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.
+#[derive(Copy, Clone)]
-const LED_COLORS: [RGB8; 11] = [
+enum LedEffect {
-    RGB8 { r: 0, g: 0, b: 0 },   // Off
+    Solid,
-    RGB8 { r: 10, g: 7, b: 0 },  // Normal (Green)
+    Blink { period_ms: u32 },
-    RGB8 { r: 10, g: 7, b: 0 },  // NormalFlash (Green)
+    Heartbeat { period_ms: u32 },
-    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)
-];
 
-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 {
+    } else if !system_state.usb_initialized {
-        StatusMode::Idle
+        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<P, SM, I>,
     current_mode: StatusMode,
-    flash_state: bool,
+    mode_started_at: Option<u32>,
     last_update_time: Option<u32>,
 }
 
@@ -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);
+        let base_mode = determine_base_mode(system_state);
-        self.set_mode(desired_mode, current_time);
+        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) {
+    pub fn update_display(&mut self, current_time: u32, force_update: bool, base_mode: StatusMode) {
-        let should_update = force_update || self.should_flash_now(current_time);
+        let descriptor = descriptor_for(self.current_mode, base_mode);
+        let interval = descriptor.effect.update_interval_ms();
 
-        if !should_update {
+        if !force_update {
-            return;
+            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);
     }
 }