AP_Notify: Add GCS failsafe notify tones and lights

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);
                 }

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)

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 {

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

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);

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 ||

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;

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