Several fixes, hex flies, failsafe not really tested yet

1 files changed, 31 insertions, 13 deletions

diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 0ad636c0b..2a087705a 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -56,18 +56,20 @@ static struct hrt_call arming_call;
  * Count the number of times in a row that we see the arming button
  * held down.
  */
-static unsigned arm_counter;
-#define ARM_COUNTER_THRESHOLD	10
+static unsigned counter;
 
-static bool safety_led_state;
+#define ARM_COUNTER_THRESHOLD	20
+#define DISARM_COUNTER_THRESHOLD	2
 
+static bool safety_led_state;
+static bool safety_button_pressed;
 static void safety_check_button(void *arg);
 
 void
 safety_init(void)
 {
-	/* arrange for the button handler to be called at 10Hz */
-	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+	/* arrange for the button handler to be called at 20Hz */
+	hrt_call_every(&arming_call, 1000, 50000, safety_check_button, NULL);
 }
 
 static void
@@ -77,18 +79,34 @@ safety_check_button(void *arg)
 	 * Debounce the safety button, change state if it has been held for long enough.
 	 *
 	 */
+	safety_button_pressed = BUTTON_SAFETY;
+
+	if(safety_button_pressed) {
+		//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
+	}
 
-	if (BUTTON_SAFETY) {
-		if (arm_counter < ARM_COUNTER_THRESHOLD) {
-			arm_counter++;
-		} else if (arm_counter == ARM_COUNTER_THRESHOLD) {
-			/* change our armed state and notify the FMU */
-			system_state.armed = !system_state.armed;
-			arm_counter++;
+	/* Keep pressed for a while to arm */
+	if (safety_button_pressed && !system_state.armed) {
+		if (counter < ARM_COUNTER_THRESHOLD) {
+			counter++;
+		} else if (counter == ARM_COUNTER_THRESHOLD) {
+			/* change to armed state and notify the FMU */
+			system_state.armed = true;
+			counter++;
+			system_state.fmu_report_due = true;
+		}
+	/* Disarm quickly */
+	} else if (safety_button_pressed && system_state.armed) {
+		if (counter < DISARM_COUNTER_THRESHOLD) {
+			counter++;
+		} else if (counter == DISARM_COUNTER_THRESHOLD) {
+			/* change to disarmed state and notify the FMU */
+			system_state.armed = false;
+			counter++;
 			system_state.fmu_report_due = true;
 		}
 	} else {
-		arm_counter = 0;
+		counter = 0;
 	}
 
 	/* when armed, toggle the LED; when safe, leave it on */