diff options
Diffstat (limited to 'apps/px4io/safety.c')
| -rw-r--r-- | apps/px4io/safety.c | 36 |
1 files changed, 36 insertions, 0 deletions
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 24fc9951a..d5bd103c1 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -51,6 +51,8 @@ #include "px4io.h" static struct hrt_call arming_call; +static struct hrt_call heartbeat_call; +static struct hrt_call failsafe_call; /* * Count the number of times in a row that we see the arming button @@ -63,13 +65,22 @@ static unsigned counter; static bool safety_led_state; static bool safety_button_pressed; + static void safety_check_button(void *arg); +static void heartbeat_blink(void *arg); +static void failsafe_blink(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 heartbeat handler to be called at 4Hz */ + hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); + + /* arrange for the failsafe blinker to be called at 8Hz */ + hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } static void @@ -117,3 +128,28 @@ safety_check_button(void *arg) } LED_SAFETY(safety_led_state); } + + +static void +heartbeat_blink(void *arg) +{ + static bool heartbeat = false; + + /* XXX add flags here that need to be frobbed by various loops */ + + LED_BLUE(heartbeat = !heartbeat); +} + +static void +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) { + failsafe = !failsafe; + } else { + failsafe = false; + } + LED_AMBER(failsafe); +}
\ No newline at end of file |