diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-30 10:03:05 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-30 10:03:05 +0100 |
commit | 142556b442b1c88ed2ede2cb9904a6a324051e71 (patch) | |
tree | 0ad4357be8b5ef9ccab6ea6c5fa125a19e8a29d9 /apps/px4io/safety.c | |
parent | b8250de1e67f63f9ca3b990e016744584a328922 (diff) | |
parent | 62a95bf8e6592b31ae7e84e53b654bc5e6b71cd1 (diff) | |
download | px4-firmware-142556b442b1c88ed2ede2cb9904a6a324051e71.tar.gz px4-firmware-142556b442b1c88ed2ede2cb9904a6a324051e71.tar.bz2 px4-firmware-142556b442b1c88ed2ede2cb9904a6a324051e71.zip |
merged
Diffstat (limited to 'apps/px4io/safety.c')
-rw-r--r-- | apps/px4io/safety.c | 42 |
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 { |