AP_NavEKF3: Fix integration of GPS yaw options

AP_NavEKF3: fix failure to arm with EK3_MAG_CAL = 7

AP_NavEKF3: Fix failure to arm when not using magnetometer

AP_NavEKF3: Reduce yaw drift when EK3_MAG_CAL = 7

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -243,8 +243,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
 
     // @Param: MAG_CAL
     // @DisplayName: Magnetometer default fusion mode
-    // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. EK3_MAG_CAL = 5 uses an external yaw sensor with simple heading fusion. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. EK3_MAG_CAL=6 uses an external yaw sensor with fallback to compass when the external sensor is not available if we are flying.
-    // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor,6:External yaw sensor with compass fallback
+    // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. EK3_MAG_CAL = 5 uses an external yaw sensor with simple heading fusion. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. EK3_MAG_CAL=6 uses an external yaw sensor with fallback to compass when the external sensor is not available if we are flying. EK3_MAG_CAL = 7 aligns the yaw angle using IMU and GPS velocity data. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: When using EKF3_MAG_CAL = 7, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK and all COMPASS_USE parameters must be set to 0.
+    // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor,6:External yaw sensor with compass fallback,7:Use IMU and GPS velocity
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT),

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -45,7 +45,7 @@ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const
     MagCal magcal = MagCal(frontend->_magCal.get());
 
     // force use of simple magnetic heading fusion for specified cores
-    if (magcal != MagCal::EXTERNAL_YAW && (frontend->_magMask & core_index)) {
+    if (magcal != MagCal::EXTERNAL_YAW && magcal != MagCal::EXTERNAL_YAW_FALLBACK && magcal != MagCal::GSF_YAW && (frontend->_magMask & core_index)) {
         return MagCal::NEVER;
     }
 
@@ -94,17 +94,16 @@ void NavEKF3_core::setWindMagStateLearningMode()
     }
 
     // Determine if learning of magnetic field states has been requested by the user
-    MagCal magCal = effective_magCal();
     bool magCalRequested =
-        ((magCal == MagCal::WHEN_FLYING) && inFlight) || // when flying
-        ((magCal == MagCal::WHEN_MANOEUVRING) && manoeuvring)  || // when manoeuvring
-        ((magCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
-        ((magCal == MagCal::EXTERNAL_YAW_FALLBACK) && inFlight) ||
-        (magCal == MagCal::ALWAYS); // all the time
+        ((effectiveMagCal == MagCal::WHEN_FLYING) && inFlight) || // when flying
+        ((effectiveMagCal == MagCal::WHEN_MANOEUVRING) && manoeuvring)  || // when manoeuvring
+        ((effectiveMagCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
+        ((effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) && inFlight) ||
+        (effectiveMagCal == MagCal::ALWAYS); // all the time
 
     // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
     // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
-    bool magCalDenied = !use_compass() || (magCal == MagCal::NEVER) || (onGround && magCal != MagCal::ALWAYS);
+    bool magCalDenied = !use_compass() || (effectiveMagCal == MagCal::NEVER) || (onGround && effectiveMagCal != MagCal::ALWAYS);
 
     // Inhibit the magnetic field calibration if not requested or denied
     bool setMagInhibit = !magCalRequested || magCalDenied;
@@ -460,17 +459,20 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const
 // return true if we should use the compass
 bool NavEKF3_core::use_compass(void) const
 {
-    return effective_magCal() != MagCal::EXTERNAL_YAW && _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
+    return effectiveMagCal != MagCal::EXTERNAL_YAW &&
+           effectiveMagCal != MagCal::GSF_YAW &&
+           _ahrs->get_compass() &&
+           _ahrs->get_compass()->use_for_yaw(magSelectIndex) &&
+           !allMagSensorsFailed;
 }
 
-// are we using an external yaw source? Needed for ahrs attitudes_consistent
+// are we using a yaw source other than the magnetomer?
 bool NavEKF3_core::using_external_yaw(void) const
 {
-    MagCal mag_cal = effective_magCal();
-    if (mag_cal != MagCal::EXTERNAL_YAW && mag_cal != MagCal::EXTERNAL_YAW_FALLBACK) {
-        return false;
+    if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || effectiveMagCal == MagCal::GSF_YAW) {
+        return AP_HAL::millis() - last_gps_yaw_fusion_ms < 5000 || AP_HAL::millis() - lastSynthYawTime_ms < 5000;
     }
-    return AP_HAL::millis() - last_gps_yaw_fusion_ms < 5000;
+    return false;
 }
 
 /*
@@ -531,7 +533,7 @@ void NavEKF3_core::checkGyroCalStatus(void)
 {
     // check delta angle bias variances
     const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
-    if (frontend->_magCal == 6) {
+    if (effective_magCal() == MagCal::GSF_YAW) {
         // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass
         // which can make this check fail
         Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]);

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -272,9 +272,58 @@ void NavEKF3_core::SelectMagFusion()
     // used for load levelling
     magFusePerformed = false;
 
+    effectiveMagCal = effective_magCal();
+
+    // Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in
+    // flight using the output from the GSF yaw estimator.
+    if (effectiveMagCal == MagCal::GSF_YAW || !use_compass()) {
+        if (!yawAlignComplete) {
+            yawAlignComplete = EKFGSF_resetMainFilterYaw();
+        }
+
+        if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
+            if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
+                // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
+                yawAngDataDelayed.type = 2;
+            } else {
+                // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
+                yawAngDataDelayed.type = 1;
+            }
+
+            float yawEKFGSF, yawVarianceEKFGSF;
+            bool canUseEKFGSF = yawEstimator->getYawData(&yawEKFGSF, &yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f));
+            if (yawAlignComplete && canUseEKFGSF) {
+                // use the EKF-GSF yaw estimator output as this is more robust that the EKF can achieve without a yaw measurement
+                yawAngDataDelayed.yawAngErr = MAX(sqrtf(yawVarianceEKFGSF), 0.05f);
+                yawAngDataDelayed.yawAng = yawEKFGSF;
+                fuseEulerYaw(false, true);
+            } else {
+                // fuse the last dead-reckoned yaw when static to stop yaw drift and estimate yaw gyro bias estimate
+                yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
+                if (!onGroundNotMoving) {
+                    if (yawAngDataDelayed.type == 2) {
+                        yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
+                    } else if (yawAngDataDelayed.type == 1) {
+                        yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
+                    }
+                }
+                if (onGroundNotMoving) {
+                    // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
+                    fuseEulerYaw(false, true);
+                } else {
+                    // prevent uncontrolled yaw variance growth by fusing a zero innovation
+                    fuseEulerYaw(true, true);
+                }
+            }
+            magTestRatio.zero();
+            yawTestRatio = 0.0f;
+            lastSynthYawTime_ms = imuSampleTime_ms;
+        }
+        return;
+    }
+
     // Handle case where we are using an external yaw sensor instead of a magnetomer
-    MagCal magcal = effective_magCal();
-    if (magcal == MagCal::EXTERNAL_YAW || magcal == MagCal::EXTERNAL_YAW_FALLBACK) {
+    if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) {
         bool have_fused_gps_yaw = false;
         uint32_t now = AP_HAL::millis();
         if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
@@ -310,7 +359,7 @@ void NavEKF3_core::SelectMagFusion()
             }
             lastSynthYawTime_ms = imuSampleTime_ms;
         }
-        if (magcal == MagCal::EXTERNAL_YAW) {
+        if (effectiveMagCal == MagCal::EXTERNAL_YAW) {
             // no fallback
             return;
         }
@@ -350,35 +399,13 @@ void NavEKF3_core::SelectMagFusion()
         // fall through to magnetometer fusion
     }
 
-    if (magcal != MagCal::EXTERNAL_YAW_FALLBACK) {
+    if (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) {
         // check for and read new magnetometer measurements. We don't
         // real for EXTERNAL_YAW_FALLBACK as it has already been read
         // above
         readMagData();
     }
-    // Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in
-    // flight using the output from the GSF yaw estimator.
-    if (frontend->_magCal == 6 || !use_compass()) {
-        if (yawAlignComplete) {
-            // Fuse a synthetic heading measurement at 7Hz to prevent the filter covariances from
-            // becoming badly conditioned when static for long periods. 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 ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) {
-                fuseEulerYaw(true, false);
-                magTestRatio.zero();
-                yawTestRatio = 0.0f;
-                lastSynthYawTime_ms = imuSampleTime_ms;
-            }
-            return;
-        }
-        yawAlignComplete = EKFGSF_resetMainFilterYaw();
-        return;
-    }
-
-    // check for and read new magnetometer measurements
-    readMagData();
-
+    
     // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
     if (magHealth) {
         magTimeout = false;
@@ -430,18 +457,6 @@ void NavEKF3_core::SelectMagFusion()
         }
     }
 
-    // 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 || !assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) {
-            fuseEulerYaw(true, false);
-            magTestRatio.zero();
-            yawTestRatio = 0.0f;
-            lastSynthYawTime_ms = imuSampleTime_ms;
-        }
-    }
-
     // If the final yaw reset has been performed and the state variances are sufficiently low
     // record that the earth field has been learned.
     if (!magFieldLearned && finalInflightMagInit) {
@@ -1410,8 +1425,6 @@ void NavEKF3_core::recordMagReset()
     yawInnovAtLastMagReset = innovYaw;
 }
 
-
-
 /*
   learn magnetometer biases from GPS yaw. Return true if the
   resulting mag vector is close enough to the one predicted by GPS
@@ -1477,6 +1490,7 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
     }
     return ok;
 }
+
 // Reset states using yaw from EKF-GSF and velocity and position from GPS
 bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
 {
@@ -1498,7 +1512,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
         EKFGSF_yaw_reset_ms = imuSampleTime_ms;
         EKFGSF_yaw_reset_count++;
 
-        if (frontend->_magCal == 6) {
+        if (effectiveMagCal == MagCal::GSF_YAW) {
             gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index);
         } else {
             gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u emergency yaw reset - mag sensor stopped",(unsigned)imu_index);

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -1063,12 +1063,15 @@ float NavEKF3_core::MagDeclination(void) const
 /*
   Update the on ground and not moving check.
   Should be called once per IMU update.
-  Only updates when on ground and when operating with an external yaw sensor
+  Only updates when on ground and when operating without a magnetometer
 */
 void NavEKF3_core::updateMovementCheck(void)
 {
-    MagCal magcal = effective_magCal();
-    if (!(magcal == MagCal::EXTERNAL_YAW || magcal == MagCal::EXTERNAL_YAW_FALLBACK) && !onGround) {
+    if (!onGround &&
+        effectiveMagCal != MagCal::EXTERNAL_YAW &&
+        effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK &&
+        effectiveMagCal != MagCal::GSF_YAW)
+    {
         onGroundNotMoving = false;
         return;
     }

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -607,10 +607,10 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
     } else {
         temp = 0.0f;
     }
-    const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
 
     // send message
-    mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, mag_max, temp, tasVar);
+    mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp, tasVar);
+
 }
 
 // report the reason for why the backend is refusing to initialise

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -411,6 +411,8 @@ void NavEKF3_core::InitialiseVariables()
     EKFGSF_yaw_reset_request_ms = 0;
     EKFGSF_yaw_reset_count = 0;
     EKFGSF_run_filterbank = false;
+
+    effectiveMagCal = effective_magCal();
 }
 
 

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -68,6 +68,7 @@
 
 // learning limit for mag biases when using GPS yaw (Gauss)
 #define EK3_GPS_MAG_LEARN_LIMIT 0.02f
+
 // maximum number of yaw resets due to detected magnetic anomaly allowed per flight
 #define MAG_ANOMALY_RESET_MAX 2
 
@@ -388,6 +389,7 @@ public:
         ALWAYS = 4,
         EXTERNAL_YAW = 5,
         EXTERNAL_YAW_FALLBACK = 6,
+        GSF_YAW = 7,
     };
 
     // are we using an external yaw source? This is needed by AHRS attitudes_consistent check
@@ -951,6 +953,7 @@ private:
     float defaultAirSpeed;          // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
     bool magFusePerformed;          // boolean set to true when magnetometer fusion has been perfomred in that time step
     bool magFuseRequired;           // boolean set to true when magnetometer fusion will be perfomred in the next time step
+    MagCal effectiveMagCal;         // the actual mag calibration and yaw fusion method being used as the default
     uint32_t prevTasStep_ms;        // time stamp of last TAS fusion step
     uint32_t prevBetaStep_ms;       // time stamp of last synthetic sideslip fusion step
     uint32_t lastMagUpdate_us;      // last time compass was updated in usec