From 36031c01a97b3b566deae052d1bb9b377e45be9e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 10 Mar 2020 17:51:53 +1100 Subject: [PATCH] ArduCopter: rework ekf check to use separate yaw reset request ArduCopter: Request lane or yaw reset switch sooner ArduCopter: rework ekf check to use separate yaw reset request ArduCopter: Rework EKF failsafe During flight operations it is the deviation in position of the vehicle that matters during most scenarios. t patch ensures that an EKF fail-safe will be triggered when the position inconsistency exceeds check limits in addition to either a bad mag or velocity innovation. This will stop velocity noise triggering an early fail-safe when electromagnetic interference is present. --- ArduCopter/ekf_check.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 0472aaa8f70..bcbd615527b 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -49,10 +49,17 @@ void Copter::ekf_check() if (!ekf_check_state.bad_variance) { // increase counter ekf_check_state.fail_count++; +#if EKF_CHECK_ITERATIONS_MAX > 3 + if (ekf_check_state.fail_count == MIN((EKF_CHECK_ITERATIONS_MAX-2), 5)) { + // we are just about to declare a EKF failsafe, ask the EKF if we can reset + // yaw to resolve the issue + ahrs.request_yaw_reset(); + } +#endif #if EKF_CHECK_ITERATIONS_MAX > 2 if (ekf_check_state.fail_count == EKF_CHECK_ITERATIONS_MAX-1) { - // we are just about to declare a EKF failsafe, ask the EKF if we can change lanes - // to resolve the issue + // we are just about to declare a EKF failsafe, ask the EKF if we can + // change lanes to resolve the issue ahrs.check_lane_switch(); } #endif @@ -112,6 +119,7 @@ bool Copter::ekf_over_threshold() if (mag_max >= g.fs_ekf_thresh) { over_thresh_count++; } + bool optflow_healthy = false; #if OPTFLOW == ENABLED optflow_healthy = optflow.healthy(); @@ -121,11 +129,8 @@ bool Copter::ekf_over_threshold() } else if (vel_variance >= g.fs_ekf_thresh) { over_thresh_count++; } - if (position_variance >= g.fs_ekf_thresh) { - over_thresh_count++; - } - if (over_thresh_count >= 2) { + if (position_variance >= g.fs_ekf_thresh && over_thresh_count >= 1) { return true; }