diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 64f398e4d4..8225359804 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -608,13 +608,20 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const switch (active_EKF_type()) { case EKFType::NONE: // EKF is secondary + switch ((EKFType)_ekf_type.get()) { +#if HAL_NAVEKF3_AVAILABLE + case EKFType::THREE: + EKF3.getEulerAngles(-1, eulers); + return _ekf3_started; +#endif #if HAL_NAVEKF2_AVAILABLE - EKF2.getEulerAngles(-1, eulers); - return _ekf2_started; -#else - return false; + case EKFType::TWO: + EKF2.getEulerAngles(-1, eulers); + return _ekf2_started; #endif - + default: + return false; + } #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: #endif