diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 6 | ||||
-rw-r--r-- | apps/px4io/safety.c | 33 |
2 files changed, 29 insertions, 10 deletions
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 5d928264d..3734d7755 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -1080,10 +1080,10 @@ int HMC5883::check_offset() int HMC5883::check_calibration() { - bool offset_valid = !(check_offset() == OK); - bool scale_valid = !(check_scale() == OK); + bool offset_valid = (check_offset() == OK); + bool scale_valid = (check_scale() == OK); - if (_calibrated != (offset_valid && scale_valid == OK)) { + if (_calibrated != (offset_valid && scale_valid)) { warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index d5bd103c1..60d20905a 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -60,10 +60,19 @@ static struct hrt_call failsafe_call; */ static unsigned counter; +/* + * 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 + +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); @@ -120,15 +129,25 @@ safety_check_button(void *arg) counter = 0; } - /* when armed, toggle the LED; 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) { - safety_led_state = !safety_led_state; - } else { - safety_led_state = true; + 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; } - 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) |