diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 4874f1483f..ce53e213fb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -528,7 +528,8 @@ bool NavEKF3_core::use_compass(void) const bool NavEKF3_core::using_external_yaw(void) const { 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()) { + if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || + yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) { return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; } if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 3ff3df46e2..00149d65d5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -219,12 +219,13 @@ void NavEKF3_core::SelectMagFusion() yawAngDataStatic.time_ms = imuDataDelayed.time_ms; } - // Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in + // 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. - if (!use_compass() && - yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL && - yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK && - yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV) { + if ((yaw_source == AP_NavEKF_Source::SourceYaw::GSF) || + (!use_compass() && + yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL && + yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK && + yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) { // because this type of reset event is not as time critical, require a continuous history of valid estimates if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) { @@ -1518,7 +1519,8 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw() EKFGSF_yaw_reset_ms = imuSampleTime_ms; EKFGSF_yaw_reset_count++; - if (!use_compass() || dal.compass().get_num_enabled() == 0) { + if ((frontend->sources.getYawSource() == 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 { GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 IMU%u emergency yaw reset",(unsigned)imu_index); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 2ba1d994ba..58e6079afe 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -1291,7 +1291,7 @@ 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::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || - yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || !use_compass()); + yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()); if (!runCheck) { onGroundNotMoving = false;