AP_Notify: add EKF failure warning

libraries/AP_Notify/AP_Notify.h

@@ -44,6 +44,7 @@ public:
         uint16_t failsafe_gps       : 1;    // 1 if gps failsafe
         uint16_t arming_failed      : 1;    // 1 if copter failed to arm after user input
         uint16_t parachute_release  : 1;    // 1 if parachute is being released
+        uint16_t ekf_bad            : 1;    // 1 if ekf is reporting problems
 
         // additional flags
         uint16_t external_leds      : 1;    // 1 if external LEDs are enabled (normally only used for copter)

libraries/AP_Notify/ToneAlarm_PX4.cpp

@@ -120,6 +120,15 @@ void ToneAlarm_PX4::update()
             play_tune(TONE_PARACHUTE_RELEASE_TUNE);
         }
     }
+
+    // check ekf has gone bad
+    if (flags.ekf_bad != AP_Notify::flags.ekf_bad) {
+        flags.ekf_bad = AP_Notify::flags.ekf_bad;
+        if (flags.ekf_bad) {
+            // ekf warning tune
+            play_tune(TONE_EKF_WARNING_TUNE);
+        }
+    }
 }
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

libraries/AP_Notify/ToneAlarm_PX4.h

@@ -43,6 +43,7 @@ private:
         uint8_t failsafe_gps       : 1;    // 1 if gps failsafe
         uint8_t arming_failed      : 1;    // 0 = failing checks, 1 = passed
         uint8_t parachute_release  : 1;    // 1 if parachute is being released
+        uint8_t ekf_bad            : 1;    // 1 if ekf position has gone bad
     } flags;
 };
 

libraries/AP_Notify/ToshibaLED.cpp

@@ -132,9 +132,12 @@ void ToshibaLED::update_colours(void)
         return;
     }
 
-    // failsafe patterns for radio and battery - single flash yellow
-    // failsafe pattern for gps - flashing blue and yellow
-    if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) {
+    // radio and battery failsafe patter: flash yellow
+    // gps failsafe pattern : flashing yellow and blue
+    // ekf_bad pattern : flashing yellow and red
+    if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery ||
+            AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching ||
+            AP_Notify::flags.ekf_bad) {
         switch(step) {
             case 0:
             case 1:
@@ -151,14 +154,22 @@ void ToshibaLED::update_colours(void)
             case 7:
             case 8:
             case 9:
-                // all off of radio or battery, blue on for gps
-                _red_des = TOSHIBA_LED_OFF;
                 if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) {
+                    // blue on for gps failsafe or glitching
+                    _red_des = TOSHIBA_LED_OFF;
                     _blue_des = brightness;
+                    _green_des = TOSHIBA_LED_OFF;
+                } else if (AP_Notify::flags.ekf_bad) {
+                    // red on if ekf bad
+                    _red_des = brightness;
+                    _blue_des = TOSHIBA_LED_OFF;
+                    _green_des = TOSHIBA_LED_OFF;
                 }else{
+                    // all off for radio or battery failsafe
+                    _red_des = TOSHIBA_LED_OFF;
                     _blue_des = TOSHIBA_LED_OFF;
+                    _green_des = TOSHIBA_LED_OFF;
                 }
-                _green_des = TOSHIBA_LED_OFF;
                 break;
         }
         // exit so no other status modify this pattern