Add yaw recovery that uses only IMU and GPS data

ArduCopter/ekf_check.cpp

@@ -50,9 +50,9 @@ void Copter::ekf_check()
             // 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 just about to declare a EKF failsafe, ask the EKF if we can reset
+                // the yaw or change lanes to resolve the issue
                 ahrs.check_lane_switch();
             }
 #endif

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_NavEKF.cpp

@@ -1405,6 +1405,13 @@ void AP_AHRS_NavEKF::writeExtNavData(const Vector3f &sensOffset, const Vector3f
 #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_NAVEKF3_AVAILABLE
+    EKF3.writeDefaultAirSpeed(airspeed);
+#endif
+}
 
 // inhibit GPS usage
 uint8_t AP_AHRS_NavEKF::setInhibitGPS(void)

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -173,6 +173,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 &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) override;
 

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