Instantly update status LED from runtime.

controller/src/led_driver.c

@@ -245,61 +245,73 @@ void led_driver_panic(void) {
     (1 << GPIO_PIN(PINDEF_YELLOW_LED));
 }
 
-static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) {
-  (void) pvParameters;
-
-  uint8_t current_led_pattern = 0;
-  uint8_t prev_led_pattern = 0;
-  uint8_t pattern_step_index = 0;
-  uint8_t current_step_ticks = 0;
-
-  while (1) {
-    current_led_pattern = led_driver_current_mode;
-
-    // New pattern --> reinit
-    if (current_led_pattern != prev_led_pattern) {
-      prev_led_pattern = current_led_pattern;
-      pattern_step_index = 0;
-      current_step_ticks = 0;
-    }
-
-    // Set/clear the appropriate LEDs
-    uint8_t led_states = driver_patterns[current_led_pattern]
-      .entries[pattern_step_index].led_states;
-    led_states &= ~led_driver_fixed_mask;
-    led_states |= led_driver_fixed_pattern & led_driver_fixed_mask;
-    if (button_driver_get_button_state(0))
-      led_states = led_driver_fixed_pattern;
-    if (button_driver_get_button_state(1)) led_states = ~led_states;
-
+static void update_hardware(uint8_t change_mask, uint8_t led_states) {
+  if (change_mask & LED_YELLOW) {
     if (led_states & LED_YELLOW) {
       GPIO_BANK(PINDEF_YELLOW_LED)->BSRRL =
         (1 << GPIO_PIN(PINDEF_YELLOW_LED));
     } else {
       GPIO_BANK(PINDEF_YELLOW_LED)->BSRRH =
         (1 << GPIO_PIN(PINDEF_YELLOW_LED));
     }
+  }
+  if (change_mask & LED_BLUE) {
     if (led_states & LED_BLUE) {
       GPIO_BANK(PINDEF_BLUE_LED)->BSRRL =
         (1 << GPIO_PIN(PINDEF_BLUE_LED));
     } else {
       GPIO_BANK(PINDEF_BLUE_LED)->BSRRH =
         (1 << GPIO_PIN(PINDEF_BLUE_LED));
     }
+  }
+  if (change_mask & LED_GREEN) {
     if (led_states & LED_GREEN) {
       GPIO_BANK(PINDEF_GREEN_LED)->BSRRL =
         (1 << GPIO_PIN(PINDEF_GREEN_LED));
     } else {
       GPIO_BANK(PINDEF_GREEN_LED)->BSRRH =
         (1 << GPIO_PIN(PINDEF_GREEN_LED));
     }
+  }
+  if (change_mask & LED_RED) {
     if (led_states & LED_RED) {
       GPIO_BANK(PINDEF_RED_LED)->BSRRL =
         (1 << GPIO_PIN(PINDEF_RED_LED));
     } else {
       GPIO_BANK(PINDEF_RED_LED)->BSRRH =
         (1 << GPIO_PIN(PINDEF_RED_LED));
     }
+  }
+}
+
+static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) {
+  (void) pvParameters;
+
+  uint8_t current_led_pattern = 0;
+  uint8_t prev_led_pattern = 0;
+  uint8_t pattern_step_index = 0;
+  uint8_t current_step_ticks = 0;
+
+  while (1) {
+    current_led_pattern = led_driver_current_mode;
+
+    // New pattern --> reinit
+    if (current_led_pattern != prev_led_pattern) {
+      prev_led_pattern = current_led_pattern;
+      pattern_step_index = 0;
+      current_step_ticks = 0;
+    }
+
+    // Set/clear the appropriate LEDs
+    uint8_t led_states = driver_patterns[current_led_pattern]
+      .entries[pattern_step_index].led_states;
+    led_states &= ~led_driver_fixed_mask;
+    led_states |= led_driver_fixed_pattern & led_driver_fixed_mask;
+    if (button_driver_get_button_state(0))
+      led_states = led_driver_fixed_pattern;
+    if (button_driver_get_button_state(1)) led_states = ~led_states;
+
+    update_hardware(0xff, led_states);
 
     // Increment tick/advance step
     if (++current_step_ticks ==
@@ -335,6 +347,7 @@ uint8_t led_driver_get_mode(void) {
 void led_driver_set_fixed(uint8_t pattern, uint8_t mask) {
   led_driver_fixed_pattern = pattern;
   led_driver_fixed_mask = mask;
+  update_hardware(led_driver_fixed_mask, led_driver_fixed_pattern);
 }
 uint8_t led_driver_get_fixed_pattern() {
   return led_driver_fixed_pattern;