AP_NavEKF3: Allow copters to fly with magnetometers disabled

Copter operation without a magnetometer is limited to constant position and relative position modes only (no GPS or range beacon fusion permitted)
Copter optical flow operation without a magnetometer is permitted.
The ability of planes to takeoff/launch without a magnetometer and align the yaw using the GPS velocity is retained.

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -180,9 +180,8 @@ void NavEKF3_core::setAidingMode()
     // Save the previous status so we can detect when it has changed
     PV_AidingModePrev = PV_AidingMode;
 
-    bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
-    bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
-    bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
+    // Check that the gyro bias variance has converged
+    checkGyroCalStatus();
 
     // Determine if we should change aiding mode
      if (PV_AidingMode == AID_NONE) {
@@ -190,9 +189,9 @@ void NavEKF3_core::setAidingMode()
         // and IMU gyro bias estimates have stabilised
         // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
         // GPS aiding is the preferred option unless excluded by the user
-        if(canUseGPS || canUseRangeBeacon) {
+        if(readyToUseGPS() || readyToUseRangeBeacon()) {
             PV_AidingMode = AID_ABSOLUTE;
-        } else if (optFlowDataPresent() && filterIsStable) {
+        } else if (readyToUseOptFlow()) {
             PV_AidingMode = AID_RELATIVE;
         }
     } else if (PV_AidingMode == AID_RELATIVE) {
@@ -202,7 +201,7 @@ void NavEKF3_core::setAidingMode()
          bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
          // Enable switch to absolute position mode if GPS or range beacon data is available
          // If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
-         if(canUseGPS || canUseRangeBeacon) {
+         if(readyToUseGPS() || readyToUseRangeBeacon()) {
              PV_AidingMode = AID_ABSOLUTE;
          } else if (flowSensorTimeout || flowFusionTimeout) {
              PV_AidingMode = AID_NONE;
@@ -305,12 +304,12 @@ void NavEKF3_core::setAidingMode()
             // Reset the last valid flow fusion time
             prevFlowFuseTime_ms = imuSampleTime_ms;
         } else if (PV_AidingMode == AID_ABSOLUTE) {
-            if (canUseGPS) {
+            if (readyToUseGPS()) {
                 // We are commencing aiding using GPS - this is the preferred method
                 posResetSource = GPS;
                 velResetSource = GPS;
                 GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
-            } else if (canUseRangeBeacon) {
+            } else if (readyToUseRangeBeacon()) {
                 // We are commencing aiding using range beacons
                 posResetSource = RNGBCN;
                 velResetSource = DEFAULT;
@@ -342,20 +341,14 @@ void NavEKF3_core::setAidingMode()
 void NavEKF3_core::checkAttitudeAlignmentStatus()
 {
     // Check for tilt convergence - used during initial alignment
-    if (norm(P[0][0],P[1][1],P[2][2],P[3][3]) < sq(0.03f) && !tiltAlignComplete) {
+    if (norm(P[0][0],P[1][1],P[2][2],P[3][3]) < sq(0.035f) && !tiltAlignComplete) {
         tiltAlignComplete = true;
         GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete\n",(unsigned)imu_index);
     }
 
-    // submit yaw and magnetic field reset requests depending on whether we have compass data
-    if (tiltAlignComplete && !yawAlignComplete) {
-        if (use_compass()) {
+    // submit yaw and magnetic field reset request depending on whether we have compass data
+    if (use_compass()&& !yawAlignComplete  && tiltAlignComplete) {
             magYawResetRequest = true;
-            gpsYawResetRequest = false;
-        } else {
-            magYawResetRequest = false;
-            gpsYawResetRequest = true;
-        }
     }
 }
 
@@ -372,22 +365,23 @@ bool NavEKF3_core::useRngFinder(void) const
     return true;
 }
 
-// return true if optical flow data is available
-bool NavEKF3_core::optFlowDataPresent(void) const
+// return true if the filter is ready to start using optical flow measurements
+bool NavEKF3_core::readyToUseOptFlow(void) const
 {
-    return (imuSampleTime_ms - flowMeaTime_ms < 200);
+    // We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow
+    return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
 }
 
 // return true if the filter to be ready to use gps
 bool NavEKF3_core::readyToUseGPS(void) const
 {
-    return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse;
+    return validOrigin && tiltAlignComplete && yawAlignComplete && delAngBiasLearned && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse && !gpsInhibit;
 }
 
 // return true if the filter to be ready to use the beacon range measurements
 bool NavEKF3_core::readyToUseRangeBeacon(void) const
 {
-    return tiltAlignComplete && yawAlignComplete && rngBcnGoodToAlign && rngBcnDataToFuse;
+    return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnGoodToAlign && rngBcnDataToFuse;
 }
 
 // return true if we should use the compass
@@ -440,15 +434,14 @@ void NavEKF3_core::recordYawReset()
     }
 }
 
-// return true and set the class variable true if the delta angle bias has been learned
-bool NavEKF3_core::checkGyroCalStatus(void)
+// set the class variable true if the delta angle bias variances are sufficiently small
+void NavEKF3_core::checkGyroCalStatus(void)
 {
     // check delta angle bias variances
     const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
     delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
                         (P[11][11] <= delAngBiasVarMax) &&
                         (P[12][12] <= delAngBiasVarMax);
-    return delAngBiasLearned;
 }
 
 // Commands the EKF to not use GPS.
@@ -477,7 +470,8 @@ void  NavEKF3_core::updateFilterStatus(void)
     bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
     bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
     bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
-    bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
+    bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
+
     // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
     bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
 

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -274,10 +274,11 @@ void NavEKF3_core::SelectMagFusion()
         }
     }
 
-    // If we have no magnetometer and are on the ground, fuse in a synthetic heading measurement to prevent the
-    // filter covariances from becoming badly conditioned
+    // If we have no magnetometer, fuse in a synthetic heading measurement at 7Hz to prevent the filter covariances
+    // from becoming badly conditioned. For planes we only do this on-ground because they can align the yaw from GPS when
+    // airborne. For other platform types we do this all the time.
     if (!use_compass()) {
-        if (onGround && (imuSampleTime_ms - lastSynthYawTime_ms > 1000)) {
+        if ((onGround || assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) {
             fuseEulerYaw();
             magTestRatio.zero();
             yawTestRatio = 0.0f;
@@ -807,7 +808,7 @@ void NavEKF3_core::fuseEulerYaw()
     // If we can't use compass data, set the  meaurement to the predicted
     // to prevent uncontrolled variance growth whilst on ground without magnetometer
     float measured_yaw;
-    if (use_compass() && yawAlignComplete && magStateInitComplete) {
+    if (use_compass() && yawAlignComplete) {
         measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
     } else {
         measured_yaw = predicted_yaw;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -620,8 +620,8 @@ private:
     // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
     void calcIMU_Weighting(float K1, float K2);
 
-    // return true if optical flow data is available
-    bool optFlowDataPresent(void) const;
+    // return true if the filter is ready to start using optical flow measurements
+    bool readyToUseOptFlow(void) const;
 
     // return true if we should use the range finder sensor
     bool useRngFinder(void) const;
@@ -667,8 +667,8 @@ private:
     // Assess GPS data quality and return true if good enough to align the EKF
     bool calcGpsGoodToAlign(void);
 
-    // return true and set the class variable true if the delta angle bias has been learned
-    bool checkGyroCalStatus(void);
+    // set the class variable true if the delta angle bias variances are sufficiently small
+    void checkGyroCalStatus(void);
 
     // update inflight calculaton that determines if GPS data is good enough for reliable navigation
     void calcGpsGoodForFlight(void);