From 3af99115fb58140b109cc1425029c948490aa4c4 Mon Sep 17 00:00:00 2001 From: Matt Lawrence Date: Thu, 31 Oct 2019 18:10:36 -0400 Subject: [PATCH] AP_Notify: Add GCS failsafe notify tones and lights --- libraries/AP_Notify/AP_BoardLED2.cpp | 2 +- libraries/AP_Notify/AP_Notify.h | 1 + libraries/AP_Notify/ExternalLED.cpp | 2 +- libraries/AP_Notify/OreoLED_I2C.cpp | 18 ++++++++++++++++++ libraries/AP_Notify/OreoLED_I2C.h | 1 + libraries/AP_Notify/RGBLed.cpp | 1 + libraries/AP_Notify/ToneAlarm.cpp | 17 +++++++++++++++++ libraries/AP_Notify/ToneAlarm.h | 1 + 8 files changed, 41 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Notify/AP_BoardLED2.cpp b/libraries/AP_Notify/AP_BoardLED2.cpp index c375c6d1ec..6673aef982 100644 --- a/libraries/AP_Notify/AP_BoardLED2.cpp +++ b/libraries/AP_Notify/AP_BoardLED2.cpp @@ -171,7 +171,7 @@ void AP_BoardLED2::update(void) if ((counter2 & 0x7) == 0) { hal.gpio->toggle(HAL_GPIO_A_LED_PIN); } - }else if(AP_Notify::flags.failsafe_radio){// blink fast (around 4Hz) + }else if(AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_gcs){// blink fast (around 4Hz) if ((counter2 & 0x3) == 0) { hal.gpio->toggle(HAL_GPIO_A_LED_PIN); } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index e85f572147..242fde02c3 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -87,6 +87,7 @@ public: bool esc_calibration; // true if calibrating escs bool failsafe_radio; // true if radio failsafe bool failsafe_battery; // true if battery failsafe + bool failsafe_gcs; // true if GCS failsafe bool parachute_release; // true if parachute is being released bool ekf_bad; // true if ekf is reporting problems bool autopilot_mode; // true if vehicle is in an autopilot flight mode (only used by OreoLEDs) diff --git a/libraries/AP_Notify/ExternalLED.cpp b/libraries/AP_Notify/ExternalLED.cpp index 7671b2be70..6085c054ac 100644 --- a/libraries/AP_Notify/ExternalLED.cpp +++ b/libraries/AP_Notify/ExternalLED.cpp @@ -185,7 +185,7 @@ void ExternalLED::update(void) break; } }else{ - if (AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_radio) { + if (AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_gcs) { // radio or battery failsafe indicated by fast flashing set_pattern(FAST_FLASH); } else { diff --git a/libraries/AP_Notify/OreoLED_I2C.cpp b/libraries/AP_Notify/OreoLED_I2C.cpp index af8622112c..7d20dac0d8 100644 --- a/libraries/AP_Notify/OreoLED_I2C.cpp +++ b/libraries/AP_Notify/OreoLED_I2C.cpp @@ -87,6 +87,10 @@ void OreoLED_I2C::update() return; // don't go any further if the Pixhawk is is in radio failsafe } + if (mode_failsafe_gcs()) { + return; // don't go any further if the Pixhawk is is in gcs failsafe + } + set_standard_colors(); if (mode_failsafe_batt()) { @@ -159,6 +163,20 @@ bool OreoLED_I2C::mode_failsafe_radio() } +// Procedure for when Pixhawk is in GCS failsafe +// LEDs perform alternating yellow X pattern +bool OreoLED_I2C::mode_failsafe_gcs() +{ + if (AP_Notify::flags.failsafe_gcs) { + set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 50, 0,0,0,0,PERIOD_SLOW,0); + set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 50, 0,0,0,0,PERIOD_SLOW,PO_ALTERNATE); + set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 50, 0,0,0,0,PERIOD_SLOW,PO_ALTERNATE); + set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 50, 0,0,0,0,PERIOD_SLOW,0); + } + return AP_Notify::flags.failsafe_gcs; +} + + // Procedure to set standard rear LED colors in aviation theme // Back LEDS White for normal, yellow for GPS not usable, purple for EKF bad] // Returns true GPS or EKF problem, returns false if all ok diff --git a/libraries/AP_Notify/OreoLED_I2C.h b/libraries/AP_Notify/OreoLED_I2C.h index 76ed1a114d..29ea1c61e9 100644 --- a/libraries/AP_Notify/OreoLED_I2C.h +++ b/libraries/AP_Notify/OreoLED_I2C.h @@ -113,6 +113,7 @@ private: bool mode_firmware_update(void); bool mode_init(void); bool mode_failsafe_radio(void); + bool mode_failsafe_gcs(void); bool set_standard_colors(void); bool mode_failsafe_batt(void); bool mode_auto_flight(void); diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp index f8799a931c..5bb9737498 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/libraries/AP_Notify/RGBLed.cpp @@ -118,6 +118,7 @@ uint32_t RGBLed::get_colour_sequence(void) const // gps failsafe pattern : flashing yellow and blue // ekf_bad pattern : flashing yellow and red if (AP_Notify::flags.failsafe_radio || + AP_Notify::flags.failsafe_gcs || AP_Notify::flags.failsafe_battery || AP_Notify::flags.ekf_bad || AP_Notify::flags.gps_glitching || diff --git a/libraries/AP_Notify/ToneAlarm.cpp b/libraries/AP_Notify/ToneAlarm.cpp index 0194254a35..1863b4d853 100644 --- a/libraries/AP_Notify/ToneAlarm.cpp +++ b/libraries/AP_Notify/ToneAlarm.cpp @@ -299,6 +299,23 @@ void AP_ToneAlarm::update() } } + // notify the user when GCS failsafe is triggered + if (flags.failsafe_gcs != AP_Notify::flags.failsafe_gcs) { + flags.failsafe_gcs = AP_Notify::flags.failsafe_gcs; + if (flags.failsafe_gcs) { + // armed case handled by events.failsafe_mode_change + if (!AP_Notify::flags.armed) { + play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK); + } + } else { + if (AP_Notify::flags.armed) { + play_tone(AP_NOTIFY_TONE_LOUD_POS_FEEDBACK); + } else { + play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK); + } + } + } + // notify the user when pre_arm checks are passing if (flags.pre_arm_check != AP_Notify::flags.pre_arm_check) { flags.pre_arm_check = AP_Notify::flags.pre_arm_check; diff --git a/libraries/AP_Notify/ToneAlarm.h b/libraries/AP_Notify/ToneAlarm.h index 86d52b57c4..ae4aaa2f1f 100644 --- a/libraries/AP_Notify/ToneAlarm.h +++ b/libraries/AP_Notify/ToneAlarm.h @@ -58,6 +58,7 @@ private: uint16_t parachute_release : 1; // 1 if parachute is being released uint16_t pre_arm_check : 1; // 0 = failing checks, 1 = passed uint16_t failsafe_radio : 1; // 1 if radio failsafe + uint16_t failsafe_gcs : 1; // 1 if gcs failsafe uint16_t vehicle_lost : 1; // 1 if lost copter tone requested uint16_t compass_cal_running : 1; // 1 if compass calibration is running uint16_t waiting_for_throw : 1; // 1 if waiting for copter throw launch