diff --git a/ArduPlane/ekf_check.cpp b/ArduPlane/ekf_check.cpp index 738f63424e..6ef7f2da00 100644 --- a/ArduPlane/ekf_check.cpp +++ b/ArduPlane/ekf_check.cpp @@ -110,7 +110,9 @@ bool Plane::ekf_over_threshold() float position_variance, vel_variance, height_variance, tas_variance; Vector3f mag_variance; Vector2f offset; - ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset); + if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset)) { + return false; + }; const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);