diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-11-11 11:55:27 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-11-11 11:55:27 -0800 |
commit | a8dfcaace27aa0abee4b3c44bffee9f94e391628 (patch) | |
tree | 95b36a23669be30709c91936c7f934172c71bd2b /apps/px4io/safety.c | |
parent | 0baca3ee316ca70fd18bf2cd5128685220e57634 (diff) | |
download | px4-firmware-a8dfcaace27aa0abee4b3c44bffee9f94e391628.tar.gz px4-firmware-a8dfcaace27aa0abee4b3c44bffee9f94e391628.tar.bz2 px4-firmware-a8dfcaace27aa0abee4b3c44bffee9f94e391628.zip |
Several fixes, hex flies, failsafe not really tested yet
Diffstat (limited to 'apps/px4io/safety.c')
-rw-r--r-- | apps/px4io/safety.c | 44 |
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 */ |