ArduPlane: Improve EKF failsafe in VTOL modes

Replicates Copter behaviour with a three step process if the EKF sustains a loss of navigation as detected by high GPS innovation test ratios:

1) Attempts a yaw reset using the GSF estimate if available
2) Attempts a lane switch
3) Falls back to a non-position mode

ArduPlane/ArduPlane.cpp

@@ -51,6 +51,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #if ADVANCED_FAILSAFE == ENABLED
     SCHED_TASK(afs_fs_check,           10,    100),
 #endif
+    SCHED_TASK(ekf_check,              10,     75),
     SCHED_TASK_CLASS(GCS,            (GCS*)&plane._gcs,       update_receive,   300,  500),
     SCHED_TASK_CLASS(GCS,            (GCS*)&plane._gcs,       update_send,      300,  500),
     SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events,          50,  150),

ArduPlane/Parameters.cpp

@@ -1271,6 +1271,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0),
 
+    // @Param: FS_EKF_THRESH
+    // @DisplayName: EKF failsafe variance threshold
+    // @Description: Allows setting the maximum acceptable compass and velocity variance used to check navigation health in VTOL modes
+    // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed
+    // @User: Advanced
+    AP_GROUPINFO("FS_EKF_THRESH", 26, ParametersG2, fs_ekf_thresh, FS_EKF_THRESHOLD_DEFAULT),
+
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -575,6 +575,8 @@ public:
     // EFI Engine Monitor
     AP_EFI efi;
 #endif
+
+    AP_Float        fs_ekf_thresh;
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -1036,6 +1036,14 @@ private:
     static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
                   "_failsafe_priorities is missing the sentinel");
 
+    // EKF checks for loss of navigation performed in ekf_check.cpp
+    // These are specific to VTOL operation
+    void ekf_check();
+    bool ekf_over_threshold();
+    void failsafe_ekf_event();
+    void failsafe_ekf_off_event(void);
+    bool ekf_position_ok() const;
+
 public:
     void failsafe_check(void);
     bool set_target_location(const Location& target_loc) override;

ArduPlane/config.h

@@ -308,3 +308,9 @@
 #ifndef LANDING_GEAR_ENABLED
  #define LANDING_GEAR_ENABLED !HAL_MINIMIZE_FEATURES
 #endif
+
+//////////////////////////////////////////////////////////////////////////////
+//  EKF Failsafe
+#ifndef FS_EKF_THRESHOLD_DEFAULT
+ # define FS_EKF_THRESHOLD_DEFAULT      0.8f    // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
+#endif

ArduPlane/ekf_check.cpp

@@ -0,0 +1,203 @@
+#include "Plane.h"
+
+/**
+ *
+ * Detects failures of the ekf or inertial nav system triggers an alert
+ * to the pilot and helps take countermeasures
+ *
+ */
+
+#ifndef EKF_CHECK_ITERATIONS_MAX
+ # define EKF_CHECK_ITERATIONS_MAX          10      // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure
+#endif
+
+#ifndef EKF_CHECK_WARNING_TIME
+ # define EKF_CHECK_WARNING_TIME            (30*1000)   // warning text messages are sent to ground no more than every 30 seconds
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// EKF_check structure
+////////////////////////////////////////////////////////////////////////////////
+static struct {
+    uint8_t fail_count;         // number of iterations ekf or dcm have been out of tolerances
+    uint8_t bad_variance : 1;   // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX)
+    uint32_t last_warn_time;    // system time of last warning in milliseconds.  Used to throttle text warnings sent to GCS
+    bool failsafe_on;           // true when the loss of navigation failsafe is on
+} ekf_check_state;
+
+// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
+// should be called at 10hz
+void Plane::ekf_check()
+{
+    // ensure EKF_CHECK_ITERATIONS_MAX is at least 7
+    static_assert(EKF_CHECK_ITERATIONS_MAX >= 7, "EKF_CHECK_ITERATIONS_MAX must be at least 7");
+
+    // exit immediately if ekf has no origin yet - this assumes the origin can never become unset
+    Location temp_loc;
+    if (!ahrs.get_origin(temp_loc)) {
+        return;
+    }
+
+    // return immediately if motors are not armed, or ekf check is disabled
+    if (!plane.arming.is_armed() || quadplane.in_vtol_posvel_mode() || (g2.fs_ekf_thresh <= 0.0f)) {
+        ekf_check_state.fail_count = 0;
+        ekf_check_state.bad_variance = false;
+        AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
+        failsafe_ekf_off_event();   // clear failsafe
+        return;
+    }
+
+    // compare compass and velocity variance vs threshold
+    if (ekf_over_threshold()) {
+        // if compass is not yet flagged as bad
+        if (!ekf_check_state.bad_variance) {
+            // increase counter
+            ekf_check_state.fail_count++;
+            if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2)) {
+                // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset
+                // yaw to resolve the issue
+                ahrs.request_yaw_reset();
+            }
+            if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-1)) {
+                // we are just about to declare a EKF failsafe, ask the EKF if we can
+                // change lanes to resolve the issue
+                ahrs.check_lane_switch();
+            }
+            // if counter above max then trigger failsafe
+            if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
+                // limit count from climbing too high
+                ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
+                ekf_check_state.bad_variance = true;
+                AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
+                // send message to gcs
+                if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
+                    gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
+                    ekf_check_state.last_warn_time = AP_HAL::millis();
+                }
+                failsafe_ekf_event();
+            }
+        }
+    } else {
+        // reduce counter
+        if (ekf_check_state.fail_count > 0) {
+            ekf_check_state.fail_count--;
+
+            // if compass is flagged as bad and the counter reaches zero then clear flag
+            if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
+                ekf_check_state.bad_variance = false;
+                AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
+                // clear failsafe
+                failsafe_ekf_off_event();
+            }
+        }
+    }
+
+    // set AP_Notify flags
+    AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
+
+    // To-Do: add ekf variances to extended status
+}
+
+// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
+bool Plane::ekf_over_threshold()
+{
+    // return false immediately if disabled
+    if (g2.fs_ekf_thresh <= 0.0f) {
+        return false;
+    }
+
+    // use EKF to get variance
+    float position_variance, vel_variance, height_variance, tas_variance;
+    Vector3f mag_variance;
+    Vector2f offset;
+    ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset);
+
+    const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);
+
+    // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold
+    uint8_t over_thresh_count = 0;
+    if (mag_max >= g2.fs_ekf_thresh) {
+        over_thresh_count++;
+    }
+
+    bool optflow_healthy = false;
+#if OPTFLOW == ENABLED
+    optflow_healthy = optflow.healthy();
+#endif
+    if (!optflow_healthy && (vel_variance >= (2.0f * g2.fs_ekf_thresh))) {
+        over_thresh_count += 2;
+    } else if (vel_variance >= g2.fs_ekf_thresh) {
+        over_thresh_count++;
+    }
+
+    if ((position_variance >= g2.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) {
+        return true;
+    }
+
+    if (ekf_position_ok()) {
+        return false;
+    }
+    return true;
+}
+
+
+// failsafe_ekf_event - perform ekf failsafe
+void Plane::failsafe_ekf_event()
+{
+    // return immediately if ekf failsafe already triggered
+    if (ekf_check_state.failsafe_on) {
+        return;
+    }
+
+    // EKF failsafe event has occurred
+    ekf_check_state.failsafe_on = true;
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
+
+    // if not in a VTOL mode requring position, then nothing needs to be done
+    if (!quadplane.in_vtol_posvel_mode()) {
+        return;
+    }
+
+    if (quadplane.in_vtol_auto()) {
+        // the pilot is not controlling via sticks so switch to QLAND
+        plane.set_mode(mode_qland, ModeReason::EKF_FAILSAFE);
+    } else {
+        // the pilot is controlling via sticks so fallbacl to QHOVER
+        plane.set_mode(mode_qhover, ModeReason::EKF_FAILSAFE);
+    }
+}
+
+// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
+void Plane::failsafe_ekf_off_event(void)
+{
+    // return immediately if not in ekf failsafe
+    if (!ekf_check_state.failsafe_on) {
+        return;
+    }
+
+    ekf_check_state.failsafe_on = false;
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
+}
+
+// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
+bool Plane::ekf_position_ok() const
+{
+    if (!ahrs.have_inertial_nav()) {
+        // do not allow navigation with dcm position
+        return false;
+    }
+
+    // with EKF use filter status and ekf check
+    nav_filter_status filt_status;
+    if(ahrs.get_filter_status(filt_status)) {
+
+    }
+
+    // if disarmed we accept a predicted horizontal position
+    if (!plane.arming.is_armed()) {
+        return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
+    } else {
+        // once armed we require a good absolute position and EKF must not be in const_pos_mode
+        return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
+    }
+}

ArduPlane/quadplane.cpp

@@ -2189,6 +2189,21 @@ bool QuadPlane::in_vtol_mode(void) const
             in_vtol_auto());
 }
 
+/*
+  are we in a VTOL mode that needs position and velocity estimates?
+ */
+bool QuadPlane::in_vtol_posvel_mode(void) const
+{
+    if (!available()) {
+        return false;
+    }
+    return (plane.control_mode == &plane.mode_qloiter ||
+            plane.control_mode == &plane.mode_qland ||
+            plane.control_mode == &plane.mode_qrtl ||
+            plane.control_mode == &plane.mode_qautotune ||
+            ((plane.control_mode == &plane.mode_guided || plane.control_mode == &plane.mode_avoidADSB) && plane.auto_state.vtol_loiter) ||
+            in_vtol_auto());
+}
 
 /*
   main landing controller. Used for landing and RTL.

ArduPlane/quadplane.h

@@ -89,6 +89,7 @@ public:
     bool verify_vtol_land(void);
     bool in_vtol_auto(void) const;
     bool in_vtol_mode(void) const;
+    bool in_vtol_posvel_mode(void) const;
     void update_throttle_hover();
 
     // vtol help for is_flying()