diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index fa6adfd8fd..878dbf03a9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -404,6 +404,9 @@ void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const // return true if offsets are valid bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const { + if (!_ahrs->get_compass()) { + return false; + } // compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited, // primary compass is valid and state variances have converged const float maxMagVar = 5E-6f;