Sub: Add ekf failsafe check

ArduSub/ArduSub.cpp

@@ -214,6 +214,8 @@ void Sub::fifty_hz_loop()
 
     failsafe_crash_check();
 
+    failsafe_ekf_check();
+
     // Update servo output
     RC_Channels::set_pwm_all();
     SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f);

ArduSub/Sub.h

@@ -278,6 +278,7 @@ private:
         uint32_t terrain_last_failure_ms;   // the most recent time terrain data access failed
         uint32_t last_battery_warn_ms; // last time a battery failsafe warning was sent to gcs
         uint32_t last_crash_warn_ms; // last time a crash warning was sent to gcs
+        uint32_t last_ekf_warn_ms; // last time an ekf warning was sent to gcs
     } failsafe;
 
     // sensor health for logging
@@ -591,6 +592,7 @@ private:
     bool manual_init(bool ignore_checks);
     void manual_run();
     void failsafe_crash_check();
+    void failsafe_ekf_check(void);
     void failsafe_battery_check(void);
     void failsafe_gcs_check();
     void failsafe_manual_control_check(void);

ArduSub/defines.h

@@ -250,7 +250,10 @@ enum RTLState {
 //////////////////////////////////////////////////////////////////////////////
 //  EKF Failsafe
 // EKF failsafe definitions (FS_EKF_ENABLE parameter)
-#define FS_EKF_ACTION_DISABLED                  1       // Disabled, not implemented yet in Sub
+#define FS_EKF_ACTION_DISABLED                0       // Disabled
+#define FS_EKF_ACTION_WARN_ONLY               1       // Send warning to gcs
+#define FS_EKF_ACTION_DISARM                  2       // Disarm
+
 
 #ifndef FS_EKF_ACTION_DEFAULT
 # define FS_EKF_ACTION_DEFAULT         FS_EKF_ACTION_DISABLED  // EKF failsafe

ArduSub/failsafe.cpp

@@ -64,6 +64,60 @@ void Sub::mainloop_failsafe_check()
     }
 }
 
+void Sub::failsafe_ekf_check(void)
+{
+    static uint32_t last_ekf_good_ms = 0;
+
+    if (g.fs_ekf_action == FS_EKF_ACTION_DISABLED) {
+        last_ekf_good_ms = AP_HAL::millis();
+        failsafe.ekf = false;
+        AP_Notify::flags.ekf_bad = false;
+        return;
+
+    }
+
+    float posVar, hgtVar, tasVar;
+    Vector3f magVar;
+    Vector2f offset;
+    float compass_variance;
+    float vel_variance;
+    ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
+    compass_variance = magVar.length();
+
+    if (compass_variance < g.fs_ekf_thresh && vel_variance < g.fs_ekf_thresh) {
+        last_ekf_good_ms = AP_HAL::millis();
+        failsafe.ekf = false;
+        AP_Notify::flags.ekf_bad = false;
+        return;;
+    }
+
+    // Bad EKF for 2 solid seconds triggers failsafe
+    if (AP_HAL::millis() < last_ekf_good_ms + 2000) {
+        failsafe.ekf = false;
+        AP_Notify::flags.ekf_bad = false;
+        return;
+    }
+
+    // Only trigger failsafe once
+    if (failsafe.ekf) {
+        return;
+    }
+
+    failsafe.ekf = true;
+    AP_Notify::flags.ekf_bad = true;
+
+    Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
+
+    if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) {
+        failsafe.last_ekf_warn_ms = AP_HAL::millis();
+        gcs_send_text(MAV_SEVERITY_WARNING, "EKF bad");
+    }
+
+    if (g.fs_ekf_action == FS_EKF_ACTION_DISARM) {
+        init_disarm_motors();
+    }
+}
+
 // Battery failsafe check
 // Check the battery voltage and remaining capacity
 void Sub::failsafe_battery_check(void)