diff options
author | px4dev <px4@purgatory.org> | 2012-12-08 14:35:32 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-12-08 14:35:32 -0800 |
commit | 154035279fbfbe14be208d5ec957089f11f6447d (patch) | |
tree | e33d0cc6fdd7cee10956954de28cea4e91db8386 | |
parent | 566012d5aaa2e24854ac823bb2de2684ef43da4a (diff) | |
parent | 03b51c69e0b01e451c24e7c1158eca254ec69765 (diff) | |
download | px4-firmware-154035279fbfbe14be208d5ec957089f11f6447d.tar.gz px4-firmware-154035279fbfbe14be208d5ec957089f11f6447d.tar.bz2 px4-firmware-154035279fbfbe14be208d5ec957089f11f6447d.zip |
Merge pull request #98 from sjwilks/px4io_safety_switch
Added more LED state logic and improve code.
-rw-r--r-- | apps/px4io/safety.c | 42 |
1 files changed, 20 insertions, 22 deletions
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 761a1e1e7..60d20905a 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -61,14 +61,18 @@ static struct hrt_call failsafe_call; static unsigned counter; /* - * Used to coordinate a special blink pattern wheb both the FMU and IO are armed. + * Define the various LED flash sequences for each system state. */ -static unsigned blink_count = 0; +#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 + +static unsigned blink_counter = 0; #define ARM_COUNTER_THRESHOLD 10 #define DISARM_COUNTER_THRESHOLD 2 -static bool safety_led_state; static bool safety_button_pressed; static void safety_check_button(void *arg); @@ -125,31 +129,25 @@ safety_check_button(void *arg) counter = 0; } - /* - * When the IO is armed, toggle the LED; when IO and FMU armed use aircraft like - * pattern (long pause then 2 fast blinks); when safe, leave it on. - */ + /* 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) { - /* FMU and IO are armed */ - if (blink_count > 9) { - safety_led_state = !safety_led_state; - } else { - safety_led_state = false; - } - if (blink_count++ == 12) { - blink_count = 0; - } + pattern = LED_PATTERN_IO_FMU_ARMED; } else { - /* Only the IO is armed so use a constant blink rate */ - safety_led_state = !safety_led_state; + pattern = LED_PATTERN_IO_ARMED; } - } else { - safety_led_state = true; + } else if (system_state.arm_ok) { + pattern = LED_PATTERN_FMU_ARMED; } - LED_SAFETY(safety_led_state); -} + /* Turn the LED on if we have a 1 at the current bit position */ + LED_SAFETY(pattern & (1 << blink_counter++)); + + if (blink_counter > 15) { + blink_counter = 0; + } +} static void heartbeat_blink(void *arg) |