aboutsummaryrefslogtreecommitdiff
path: root/apps/px4io/safety.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/px4io/safety.c')
-rw-r--r--apps/px4io/safety.c22
1 files changed, 16 insertions, 6 deletions
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 60d20905a..93596ca75 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -31,9 +31,9 @@
*
****************************************************************************/
- /**
- * @file Safety button logic.
- */
+/**
+ * @file Safety button logic.
+ */
#include <nuttx/config.h>
#include <stdio.h>
@@ -95,13 +95,13 @@ safety_init(void)
static void
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) {
+ if (safety_button_pressed) {
//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
}
@@ -109,34 +109,42 @@ safety_check_button(void *arg)
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 */
+
+ /* 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 {
counter = 0;
}
/* 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) {
pattern = LED_PATTERN_IO_FMU_ARMED;
+
} else {
pattern = LED_PATTERN_IO_ARMED;
}
+
} else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED;
}
@@ -167,8 +175,10 @@ failsafe_blink(void *arg)
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) {
failsafe = !failsafe;
+
} else {
failsafe = false;
}
+
LED_AMBER(failsafe);
} \ No newline at end of file