diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 60437489b4..87140b193f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -403,6 +403,10 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const // return true if offsets are valid bool NavEKF2_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;