From 2e56e10d9c48b3d79480c8bc3260330b4a0eb369 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 7 Jul 2017 17:17:40 +0900 Subject: [PATCH] Copter: ekf check adds position variance check two of three of compass, velocity and position variances being high will trigger the ekf failsafe --- ArduCopter/ekf_check.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index ece5534883..70491b38c7 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -100,16 +100,24 @@ bool Copter::ekf_over_threshold() } // use EKF to get variance - float posVar, hgtVar, tasVar; - Vector3f magVar; + float position_variance, vel_variance, height_variance, tas_variance; + Vector3f mag_variance; Vector2f offset; - float compass_variance; - float vel_variance; - ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset); - compass_variance = magVar.length(); + ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset); - // return true if compass and velocity variance over the threshold - return (compass_variance >= g.fs_ekf_thresh && vel_variance >= g.fs_ekf_thresh); + // return true if two of compass, velocity and position variances are over the threshold + uint8_t over_thresh_count = 0; + if (mag_variance.length() >= g.fs_ekf_thresh) { + over_thresh_count++; + } + if (vel_variance >= g.fs_ekf_thresh) { + over_thresh_count++; + } + if (position_variance >= g.fs_ekf_thresh) { + over_thresh_count++; + } + + return (over_thresh_count >= 2); }