From 95f896b5565fc3c59ad8a6eb8e28d74cd5b77c3c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Apr 2020 16:37:36 +1100 Subject: [PATCH] Copter: make EKF mag variance check use max this makes it consistent with logged value --- ArduCopter/ekf_check.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index a69ae93b0a..cfb5578388 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -105,9 +105,11 @@ bool Copter::ekf_over_threshold() Vector2f offset; ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset); + const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z); + // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold uint8_t over_thresh_count = 0; - if (mag_variance.length() >= g.fs_ekf_thresh) { + if (mag_max >= g.fs_ekf_thresh) { over_thresh_count++; } if (!optflow.healthy() && (vel_variance >= (2.0f * g.fs_ekf_thresh))) {