diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 30f311d31c..5d6c10f71e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -537,16 +537,16 @@ 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 EK3_FEATURE_EXTERNAL_NAV const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); - 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()) { - return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; - } +#if EK3_FEATURE_EXTERNAL_NAV if (yaw_source == 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()) { + return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; + } return false; }