From d7ab6652daed16b440bdb256cb3cf4271ce1a082 Mon Sep 17 00:00:00 2001
From: Kyle Zentner <zentner.kyle@gmail.com>
Date: Mon, 24 Nov 2014 15:47:33 -0800
Subject: [PATCH] Instantly update status LED from runtime.

---
 controller/src/led_driver.c | 67 ++++++++++++++++++++++---------------
 1 file changed, 40 insertions(+), 27 deletions(-)

diff --git a/controller/src/led_driver.c b/controller/src/led_driver.c
index f08c6330..9ca99222 100755
--- a/controller/src/led_driver.c
+++ b/controller/src/led_driver.c
@@ -245,33 +245,8 @@ 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));
@@ -279,6 +254,8 @@ static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) {
       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));
@@ -286,6 +263,8 @@ static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) {
       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));
@@ -293,6 +272,8 @@ static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) {
       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));
@@ -300,6 +281,37 @@ static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) {
       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;