AP_NavEKF3: integrate Source for yaw

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -233,7 +233,7 @@ 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. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting all COMPASS_USE, COMPASS_USE2, COMPASS_USE3, etc parameters to 0 and setting COMPASS_ENABLE to 0. If this is done, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK. A yaw angle derived from IMU and GPS velocity data using a Gaussian Sum Filter (GSF) will then be used to align the yaw when flight commences and there is sufficient movement.
-    // @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
+    // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT),

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -38,14 +38,12 @@ void NavEKF3_core::controlFilterModes()
  */
 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) && (magcal != MagCal::EXTERNAL_YAW_FALLBACK) && (frontend->_magMask & core_index)) {
+    if (frontend->_magMask & core_index) {
         return MagCal::NEVER;
     }
 
-    return magcal;
+    return MagCal(frontend->_magCal.get());
 }
 
 // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
@@ -94,7 +92,6 @@ void NavEKF3_core::setWindMagStateLearningMode()
         ((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,
@@ -483,8 +480,14 @@ bool NavEKF3_core::readyToUseExtNav(void) const
 bool NavEKF3_core::use_compass(void) const
 {
     const auto *compass  = dal.get_compass();
-    return effectiveMagCal != MagCal::EXTERNAL_YAW &&
-           compass &&
+    const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource();
+    if ((yaw_source != AP_NavEKF_Source::SourceYaw::COMPASS) &&
+        (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
+        // not using compass as a yaw source
+        return false;
+    }
+
+    return compass &&
            compass->use_for_yaw(magSelectIndex) &&
            !allMagSensorsFailed;
 }
@@ -492,7 +495,8 @@ bool NavEKF3_core::use_compass(void) const
 // are we using a yaw source other than the magnetomer?
 bool NavEKF3_core::using_external_yaw(void) const
 {
-    if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass()) {
+    const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource();
+    if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass()) {
         return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
     }
     return false;
@@ -558,7 +562,8 @@ void NavEKF3_core::checkGyroCalStatus(void)
 {
     // check delta angle bias variances
     const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
-    if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW) && (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK)) {
+    const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource();
+    if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
         // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
         // 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

@@ -199,17 +199,23 @@ void NavEKF3_core::SelectMagFusion()
     // used for load levelling
     magFusePerformed = false;
 
-    effectiveMagCal = effective_magCal();
+    // get default yaw source
+    const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource();
+    if (yaw_source != yaw_source_last) {
+        yaw_source_last = yaw_source;
+        yaw_source_reset = true;
+    }
 
     // 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 (!use_compass() &&
-        effectiveMagCal != MagCal::EXTERNAL_YAW &&
-        effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) {
+        yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL &&
+        yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
 
         // because this type of reset event is not as time critical, require a continuous history of valid estimates
-        if (!yawAlignComplete && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
+        if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
             yawAlignComplete = EKFGSF_resetMainFilterYaw();
+            yaw_source_reset = false;
         }
 
         if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
@@ -261,11 +267,12 @@ void NavEKF3_core::SelectMagFusion()
     }
 
     // Handle case where we are using an external yaw sensor instead of a magnetomer
-    if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) {
+    if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
         bool have_fused_gps_yaw = false;
         if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
-            if (tiltAlignComplete && !yawAlignComplete) {
+            if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
                 alignYawAngle();
+                yaw_source_reset = false;
             } else if (tiltAlignComplete && yawAlignComplete) {
                 fuseEulerYaw(false, true);
             }
@@ -296,7 +303,7 @@ void NavEKF3_core::SelectMagFusion()
             }
             lastSynthYawTime_ms = imuSampleTime_ms;
         }
-        if (effectiveMagCal == MagCal::EXTERNAL_YAW) {
+        if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL) {
             // no fallback
             return;
         }
@@ -344,9 +351,9 @@ void NavEKF3_core::SelectMagFusion()
         magTimeout = true;
     }
 
-    if (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) {
+    if (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
         // check for and read new magnetometer measurements. We don't
-        // real for EXTERNAL_YAW_FALLBACK as it has already been read
+        // read for EXTERNAL_COMPASS_FALLBACK as it has already been read
         // above
         readMagData();
     }
@@ -356,6 +363,10 @@ void NavEKF3_core::SelectMagFusion()
 
     // Control reset of yaw and magnetic field states if we are using compass data
     if (magDataToFuse) {
+        if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS)) {
+            magYawResetRequest = true;
+            yaw_source_reset = false;
+        }
         controlMagYawReset();
     }
 

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -1279,7 +1279,8 @@ float NavEKF3_core::MagDeclination(void) const
 */
 void NavEKF3_core::updateMovementCheck(void)
 {
-    const bool runCheck = onGround && (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass());
+    const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource();
+    const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass());
     if (!runCheck)
     {
         onGroundNotMoving = false;

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -534,8 +534,9 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     ResetPosition(resetDataSource::DEFAULT);
     ResetHeight();
 
-    // initialise the position source
+    // initialise sources
     pos_source_last = frontend->_sources.getPosXYSource();
+    yaw_source_last = frontend->_sources.getYawSource();
 
     // define Earth rotation vector in the NED navigation frame
     calcEarthRateNED(earthRateNED, dal.get_home().lat);

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -418,9 +418,7 @@ public:
         WHEN_MANOEUVRING = 1,
         NEVER = 2,
         AFTER_FIRST_CLIMB = 3,
-        ALWAYS = 4,
-        EXTERNAL_YAW = 5,
-        EXTERNAL_YAW_FALLBACK = 6,
+        ALWAYS = 4
     };
 
     // are we using an external yaw source? This is needed by AHRS attitudes_consistent check
@@ -1025,7 +1023,7 @@ private:
     ftype varInnovVtas;             // innovation variance output from fusion of airspeed measurements
     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
-    MagCal effectiveMagCal;         // the actual mag calibration and yaw fusion method being used as the default
+    MagCal effectiveMagCal;         // the actual mag calibration 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
@@ -1477,5 +1475,6 @@ private:
     // source reset handling
     AP_NavEKF_Source::SourceXY pos_source_last;   // position source on previous iteration (used to detect a changes)
     bool pos_source_reset;                  // true when the position source has changed but the position has not yet been reset
-
+    AP_NavEKF_Source::SourceYaw yaw_source_last;    // yaw source on previous iteration (used to detect a change)
+    bool yaw_source_reset;                          // true when the yaw source has changed but the yaw has not yet been reset
 };