Copter: integrate EKF check

Check runs at 10hz
Log ekf check failures and ekd failsafes

ArduCopter/ArduCopter.pde

@@ -413,6 +413,7 @@ static struct {
     uint8_t battery             : 1; // 2   // A status flag for the battery failsafe
     uint8_t gps                 : 1; // 3   // A status flag for the gps failsafe
     uint8_t gcs                 : 1; // 4   // A status flag for the ground station failsafe
+    uint8_t ekf                 : 1; // 5   // true if ekf failsafe has occurred
 
     int8_t radio_counter;                  // number of iterations with throttle below throttle_fs_value
 
@@ -786,6 +787,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 #endif
     { update_notify,         8,     10 },
     { one_hz_loop,         400,     42 },
+    { ekf_check,            40,      2 },
     { crash_check,          40,      2 },
     { gcs_check_input,	     8,    550 },
     { gcs_send_heartbeat,  400,    150 },
@@ -850,6 +852,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 #endif
     { update_notify,         2,     100 },
     { one_hz_loop,         100,     420 },
+    { ekf_check,            10,      20 },
     { crash_check,          10,      20 },
     { gcs_check_input,	     2,     550 },
     { gcs_send_heartbeat,  100,     150 },

ArduCopter/GCS_Mavlink.pde

@@ -46,7 +46,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
     uint32_t custom_mode = control_mode;
 
     // set system as critical if any failsafe have triggered
-    if (failsafe.radio || failsafe.battery || failsafe.gps || failsafe.gcs)  {
+    if (failsafe.radio || failsafe.battery || failsafe.gps || failsafe.gcs || failsafe.ekf)  {
         system_status = MAV_STATE_CRITICAL;
     }
     

ArduCopter/defines.h

@@ -350,6 +350,8 @@ enum FlipState {
 #define ERROR_SUBSYSTEM_FLIP                13
 #define ERROR_SUBSYSTEM_AUTOTUNE            14
 #define ERROR_SUBSYSTEM_PARACHUTE           15
+#define ERROR_SUBSYSTEM_EKF_CHECK           16
+#define ERROR_SUBSYSTEM_FAILSAFE_EKF        17
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
@@ -373,6 +375,9 @@ enum FlipState {
 #define ERROR_CODE_AUTOTUNE_BAD_GAINS       2
 // parachute failed to deploy because of low altitude
 #define ERROR_CODE_PARACHUTE_TOO_LOW        2
+// EKF check definitions
+#define ERROR_CODE_EKF_CHECK_BAD_COMPASS    2
+#define ERROR_CODE_EKF_CHECK_BAD_COMPASS_CLEARED    0
 
 // Arming Check Enable/Disable bits
 #define ARMING_CHECK_NONE                   0x00

ArduCopter/system.pde

@@ -321,7 +321,8 @@ static void startup_ground(bool force_gyro_cal)
 static bool GPS_ok()
 {
     if (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
-        !gps_glitch.glitching() && !failsafe.gps) {
+        !gps_glitch.glitching() && !failsafe.gps &&
+        !ekf_check_state.bad_compass && !failsafe.ekf) {
         return true;
     }else{
         return false;