merged

1 files changed, 28 insertions, 14 deletions

diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 93596ca75..3e02b717d 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -58,20 +58,27 @@ static struct hrt_call failsafe_call;
  * Count the number of times in a row that we see the arming button
  * held down.
  */
-static unsigned counter;
+static unsigned counter = 0;
 
 /*
  * Define the various LED flash sequences for each system state.
  */
-#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
+#define LED_PATTERN_SAFE 			0xffff		/**< always on 				*/
+#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 	0xFFFE		/**< always on with short break 	*/
+#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;
 
+/*
+ * IMPORTANT: The arming state machine critically
+ * 	      depends on using the same threshold
+ *            for arming and disarming. Since disarming
+ *            is quite deadly for the system, a similar
+ *            length can be justified.
+ */
 #define ARM_COUNTER_THRESHOLD	10
-#define DISARM_COUNTER_THRESHOLD	2
 
 static bool safety_button_pressed;
 
@@ -101,12 +108,16 @@ safety_check_button(void *arg)
 	 */
 	safety_button_pressed = BUTTON_SAFETY;
 
-	if (safety_button_pressed) {
-		//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
-	}
-
-	/* Keep pressed for a while to arm */
+	/*
+	 * Keep pressed for a while to arm.
+	 *
+	 * Note that the counting sequence has to be same length
+	 * for arming / disarming in order to end up as proper
+	 * state machine, keep ARM_COUNTER_THRESHOLD the same
+	 * length in all cases of the if/else struct below.
+	 */
 	if (safety_button_pressed && !system_state.armed) {
+
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
@@ -120,10 +131,11 @@ safety_check_button(void *arg)
 		/* Disarm quickly */
 
 	} else if (safety_button_pressed && system_state.armed) {
-		if (counter < DISARM_COUNTER_THRESHOLD) {
+
+		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
-		} else if (counter == DISARM_COUNTER_THRESHOLD) {
+		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
 			system_state.armed = false;
 			counter++;
@@ -147,6 +159,8 @@ safety_check_button(void *arg)
 
 	} else if (system_state.arm_ok) {
 		pattern = LED_PATTERN_FMU_ARMED;
+	} else if (system_state.vector_flight_mode_ok) {
+		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
 	}
 
 	/* Turn the LED on if we have a 1 at the current bit position */
@@ -173,7 +187,7 @@ failsafe_blink(void *arg)
 	static bool failsafe = false;
 
 	/* blink the failsafe LED if we don't have FMU input */
-	if (!system_state.mixer_use_fmu) {
+	if (!system_state.mixer_fmu_available) {
 		failsafe = !failsafe;
 
 	} else {