diff options
Diffstat (limited to 'apps/px4io/safety.c')
-rw-r--r-- | apps/px4io/safety.c | 22 |
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 |