diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index d2179f348c..33a796b2ba 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -209,6 +209,24 @@ void NavEKF3_core::updateStateIndexLim() } } +// set the default yaw source +void NavEKF3_core::setYawSource() +{ + AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); + if (wasLearningCompass_ms > 0) { + // can't use compass while it is being calibrated + if (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS) { + yaw_source = AP_NavEKF_Source::SourceYaw::NONE; + } else if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) { + yaw_source = AP_NavEKF_Source::SourceYaw::GPS; + } + } + if (yaw_source != yaw_source_last) { + yaw_source_last = yaw_source; + yaw_source_reset = true; + } +} + // Set inertial navigation aiding mode void NavEKF3_core::setAidingMode() { @@ -218,6 +236,8 @@ void NavEKF3_core::setAidingMode() // Save the previous status so we can detect when it has changed PV_AidingModePrev = PV_AidingMode; + setYawSource(); + // Check that the gyro bias variance has converged checkGyroCalStatus(); @@ -583,9 +603,8 @@ bool NavEKF3_core::readyToUseExtNav(void) const // return true if we should use the compass bool NavEKF3_core::use_compass(void) const { - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); - if ((yaw_source != AP_NavEKF_Source::SourceYaw::COMPASS) && - (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) { + if ((yaw_source_last != AP_NavEKF_Source::SourceYaw::COMPASS) && + (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) { // not using compass as a yaw source return false; } @@ -598,14 +617,13 @@ bool NavEKF3_core::use_compass(void) const // are we using (aka fusing) a non-compass yaw? bool NavEKF3_core::using_noncompass_for_yaw(void) const { - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); #if EK3_FEATURE_EXTERNAL_NAV - if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) { + if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) { return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000)); } #endif - if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || - yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) { + if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || + yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) { return imuSampleTime_ms - last_gps_yaw_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; } return false; @@ -615,7 +633,7 @@ bool NavEKF3_core::using_noncompass_for_yaw(void) const bool NavEKF3_core::using_extnav_for_yaw() const { #if EK3_FEATURE_EXTERNAL_NAV - if (frontend->sources.getYawSource() == AP_NavEKF_Source::SourceYaw::EXTNAV) { + if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) { return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000)); } #endif @@ -685,9 +703,8 @@ void NavEKF3_core::checkGyroCalStatus(void) { // check delta angle bias variances const ftype delAngBiasVarMax = sq(radians(0.15 * dtEkfAvg)); - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); - if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) && - (yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) { + if (!use_compass() && (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) && + (yaw_source_last != AP_NavEKF_Source::SourceYaw::EXTNAV)) { // 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 const Vector3F delAngBiasVarVec { P[10][10], P[11][11], P[12][12] }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 7d02dcec6f..e8b635e560 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -158,7 +158,7 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) if (badMagYaw) { // attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches // by default fly forward vehicles use ground course for initial yaw unless the GSF is explicitly selected as the yaw source - const bool useGSF = !assume_zero_sideslip() || (frontend->sources.getYawSource() == AP_NavEKF_Source::SourceYaw::GSF); + const bool useGSF = !assume_zero_sideslip() || (yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF); if (useGSF && EKFGSF_resetMainFilterYaw(emergency_reset)) { return; } @@ -221,13 +221,6 @@ void NavEKF3_core::SelectMagFusion() // used for load levelling magFusePerformed = false; - // 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; - } - // Store yaw angle when moving for use as a static reference when not moving if (!onGroundNotMoving) { if (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) { @@ -245,13 +238,13 @@ void NavEKF3_core::SelectMagFusion() // Handle case where we are not using a yaw sensor of any type and attempt to reset the yaw in // flight using the output from the GSF yaw estimator or GPS ground course. - if ((yaw_source == AP_NavEKF_Source::SourceYaw::GSF) || + if ((yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) || (!use_compass() && - yaw_source != AP_NavEKF_Source::SourceYaw::GPS && - yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK && - yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) { + yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS && + yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK && + yaw_source_last != AP_NavEKF_Source::SourceYaw::EXTNAV)) { - if ((!yawAlignComplete || yaw_source_reset) && ((yaw_source != AP_NavEKF_Source::SourceYaw::GSF) || (EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD))) { + if ((!yawAlignComplete || yaw_source_reset) && ((yaw_source_last != AP_NavEKF_Source::SourceYaw::GSF) || (EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD))) { realignYawGPS(false); yaw_source_reset = false; } else { @@ -262,7 +255,7 @@ void NavEKF3_core::SelectMagFusion() // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement // for non fixed wing platform types ftype gsfYaw, gsfYawVariance; - const bool didUseEKFGSF = yawAlignComplete && (yaw_source == AP_NavEKF_Source::SourceYaw::GSF) && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF); + const bool didUseEKFGSF = yawAlignComplete && (yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF); // fallback methods if (!didUseEKFGSF) { @@ -283,7 +276,7 @@ void NavEKF3_core::SelectMagFusion() } // Handle case where we are using GPS yaw sensor instead of a magnetomer - if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) { + if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) { bool have_fused_gps_yaw = false; if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) { if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) { @@ -319,7 +312,7 @@ void NavEKF3_core::SelectMagFusion() yaw_source_reset = true; } - if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS) { + if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS) { // no fallback return; } @@ -362,7 +355,7 @@ void NavEKF3_core::SelectMagFusion() #if EK3_FEATURE_EXTERNAL_NAV // Handle case where we are using an external nav for yaw const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms); - if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) { + if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) { if (extNavYawDataToFuse) { if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) { alignYawAngle(extNavYawAngDataDelayed); @@ -397,7 +390,7 @@ void NavEKF3_core::SelectMagFusion() magTimeout = true; } - if (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) { + if (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) { // check for and read new magnetometer measurements. We don't // read for GPS_COMPASS_FALLBACK as it has already been read // above @@ -409,8 +402,8 @@ 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 || - yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) { + if (yaw_source_reset && (yaw_source_last == AP_NavEKF_Source::SourceYaw::COMPASS || + yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) { magYawResetRequest = true; yaw_source_reset = false; } @@ -1573,7 +1566,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset) EKFGSF_yaw_reset_ms = imuSampleTime_ms; EKFGSF_yaw_reset_count++; - if ((frontend->sources.getYawSource() == AP_NavEKF_Source::SourceYaw::GSF) || + if ((yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) || !use_compass() || (dal.compass().get_num_enabled() == 0)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 9ebe777515..258bd6bcf5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -295,16 +295,10 @@ void NavEKF3_core::readMagData() } if (compass.learn_offsets_enabled()) { - // while learning offsets keep all mag states reset - InitialiseVariablesMag(); wasLearningCompass_ms = imuSampleTime_ms; } else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) { + // allow time for old data to clear the buffer before signalling other code that compass data can be used wasLearningCompass_ms = 0; - // force a new yaw alignment 1s after learning completes. The - // delay is to ensure any buffered mag samples are discarded - yawAlignComplete = false; - yawAlignGpsValidCount = 0; - InitialiseVariablesMag(); } // If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available @@ -1329,9 +1323,8 @@ ftype NavEKF3_core::MagDeclination(void) const */ void NavEKF3_core::updateMovementCheck(void) { - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); - const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || - yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()); + const bool runCheck = onGround && (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || + yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV || yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()); if (!runCheck) { onGroundNotMoving = false; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index e1903a7faf..2b5138062f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -886,6 +886,9 @@ private: // Determine if we are flying or on the ground void detectFlight(); + // set the default yaw source + void setYawSource(); + // Set inertial navigation aiding mode void setAidingMode();