EKF2 and EKF3 operation with no or bad compass data

ArduCopter/ekf_check.cpp

@@ -28,6 +28,9 @@ static struct {
 // should be called at 10hz
 void Copter::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)) {
@@ -49,13 +52,16 @@ void Copter::ekf_check()
         if (!ekf_check_state.bad_variance) {
             // increase counter
             ekf_check_state.fail_count++;
-#if EKF_CHECK_ITERATIONS_MAX > 2
-            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
+            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();
             }
-#endif
             // if counter above max then trigger failsafe
             if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
                 // limit count from climbing too high
@@ -112,6 +118,7 @@ bool Copter::ekf_over_threshold()
     if (mag_max >= g.fs_ekf_thresh) {
         over_thresh_count++;
     }
+
     bool optflow_healthy = false;
 #if OPTFLOW == ENABLED
     optflow_healthy = optflow.healthy();
@@ -121,11 +128,8 @@ bool Copter::ekf_over_threshold()
     } else if (vel_variance >= g.fs_ekf_thresh) {
         over_thresh_count++;
     }
-    if (position_variance >= g.fs_ekf_thresh) {
-        over_thresh_count++;
-    }
 
-    if (over_thresh_count >= 2) {
+    if ((position_variance >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) {
         return true;
     }
 

ArduPlane/ArduPlane.cpp

@@ -262,6 +262,8 @@ void Plane::one_second_loop()
     adsb.set_stall_speed_cm(aparm.airspeed_min);
     adsb.set_max_speed(aparm.airspeed_max);
 
+    ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2));
+
     // sync MAVLink system ID
     mavlink_system.sysid = g.sysid_this_mav;
 

libraries/AP_AHRS/AP_AHRS.h

@@ -186,6 +186,9 @@ class AP_AHRS
 
     // check whether external navigation is providing yaw.  Allows compass pre-arm checks to be bypassed
     virtual bool is_ext_nav_used_for_yaw(void) const { return false; }
+
+    // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
+    virtual void request_yaw_reset(void) {}
 
     // Euler angles (radians)
     float roll;

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -1455,6 +1455,16 @@ void AP_AHRS_NavEKF::writeExtNavData(const Vector3f &pos, const Quaternion &quat
 #endif
 }
 
+// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
+void AP_AHRS_NavEKF::writeDefaultAirSpeed(float airspeed)
+{
+#if HAL_NAVEKF2_AVAILABLE
+    EKF2.writeDefaultAirSpeed(airspeed);
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+    EKF3.writeDefaultAirSpeed(airspeed);
+#endif
+}
 
 // inhibit GPS usage
 uint8_t AP_AHRS_NavEKF::setInhibitGPS(void)
@@ -2186,6 +2196,32 @@ void AP_AHRS_NavEKF::check_lane_switch(void)
     }
 }
 
+// request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
+void AP_AHRS_NavEKF::request_yaw_reset(void)
+{
+    switch (active_EKF_type()) {
+    case EKFType::NONE:
+        break;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKFType::SITL:
+        break;
+#endif
+
+#if HAL_NAVEKF2_AVAILABLE
+    case EKFType::TWO:
+        EKF2.requestYawReset();
+        break;
+#endif
+
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+        EKF3.requestYawReset();
+        break;
+#endif
+    }
+}
+
 void AP_AHRS_NavEKF::Log_Write()
 {
 #if HAL_NAVEKF2_AVAILABLE
@@ -2212,6 +2248,7 @@ bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const
     case EKFType::NONE:
 #if HAL_NAVEKF3_AVAILABLE
     case EKFType::THREE:
+        return EKF3.using_external_yaw();
 #endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     case EKFType::SITL:

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -179,6 +179,9 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM {
     // write body odometry measurements to the EKF
     void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset);
 
+    // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
+    void writeDefaultAirSpeed(float airspeed);
+
     // Write position and quaternion data from an external navigation system
     void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) override;
 
@@ -284,6 +287,9 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM {
     // see if EKF lane switching is possible to avoid EKF failsafe
     void check_lane_switch(void) override;
 
+    // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
+    void request_yaw_reset(void) override;
+
     void Log_Write();
 
     // check whether external navigation is providing yaw.  Allows compass pre-arm checks to be bypassed

libraries/AP_NavEKF/AP_Nav_Common.h

@@ -77,3 +77,5 @@ struct ekf_timing {
     float delVelDT_min;
 };
 void Log_EKF_Timing(const char *name, const uint8_t core, uint64_t time_us, const struct ekf_timing &timing);
+
+#define N_MODELS_EKFGSF 5U