Added more LED state logic and improve code.

The LED will now also indicate when the FMU is ARMED. Switched to using
a 16-bit value where each bit indicates what state the LED should be
in.

1 files changed, 20 insertions, 22 deletions

diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 761a1e1e7..60d20905a 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -61,14 +61,18 @@ static struct hrt_call failsafe_call;
 static unsigned counter;
 
 /*
- * Used to coordinate a special blink pattern wheb both the FMU and IO are armed.
+ * Define the various LED flash sequences for each system state.
  */
-static unsigned blink_count = 0;
+#define LED_PATTERN_SAFE 			0xffff		// always on
+#define LED_PATTERN_FMU_ARMED 		0x4444		// slow blinking
+#define LED_PATTERN_IO_ARMED 		0x5555		// fast blinking 
+#define LED_PATTERN_IO_FMU_ARMED 	0x5050		// long off then double blink
+
+static unsigned blink_counter = 0;
 
 #define ARM_COUNTER_THRESHOLD	10
 #define DISARM_COUNTER_THRESHOLD	2
 
-static bool safety_led_state;
 static bool safety_button_pressed;
 
 static void safety_check_button(void *arg);
@@ -125,31 +129,25 @@ safety_check_button(void *arg)
 		counter = 0;
 	}
 
-	/* 
-	 * When the IO is armed, toggle the LED; when IO and FMU armed use aircraft like 
-	 * pattern (long pause then 2 fast blinks); when safe, leave it on. 
-	 */
+	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
+	uint16_t pattern = LED_PATTERN_SAFE;
 	if (system_state.armed) {
 		if (system_state.arm_ok) {
-			/* FMU and IO are armed */
-			if (blink_count > 9) {
-				safety_led_state = !safety_led_state;
-			} else {
-				safety_led_state = false;
-			}
-			if (blink_count++ == 12) {
-				blink_count = 0;
-			}
+			pattern = LED_PATTERN_IO_FMU_ARMED;
 		} else {
-			/* Only the IO is armed so use a constant blink rate */
-			safety_led_state = !safety_led_state;
+			pattern = LED_PATTERN_IO_ARMED;
 		}
-	} else {
-		safety_led_state = true;
+	} else if (system_state.arm_ok) {
+		pattern = LED_PATTERN_FMU_ARMED;
 	}
-	LED_SAFETY(safety_led_state);
-}
 
+	/* Turn the LED on if we have a 1 at the current bit position */
+	LED_SAFETY(pattern & (1 << blink_counter++));
+
+	if (blink_counter > 15) {
+		blink_counter = 0;
+	}
+}
 
 static void
 heartbeat_blink(void *arg)