From 379506b718bc4719504478d9da979becba666620 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Wed, 28 Oct 2020 20:55:21 +1100
Subject: [PATCH] ArduCopter: Don't request EKF yaw reset unless innovations
 are large

This prevents unwanted resets if GPS fails.
---
 ArduCopter/Copter.h               |  4 ++--
 ArduCopter/baro_ground_effect.cpp |  6 +++---
 ArduCopter/ekf_check.cpp          | 14 ++++++--------
 ArduCopter/system.cpp             | 10 +++++-----
 4 files changed, 16 insertions(+), 18 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 785a1c17cf..e5434d2d06 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -859,8 +859,8 @@ private:
     void startup_INS_ground();
     void update_dynamic_notch() override;
     bool position_ok() const;
-    bool ekf_position_ok() const;
-    bool optflow_position_ok() const;
+    bool ekf_has_absolute_position() const;
+    bool ekf_has_relative_position() const;
     bool ekf_alt_ok() const;
     void update_auto_armed();
     bool should_log(uint32_t mask);
diff --git a/ArduCopter/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp
index 0c023b927e..20488a3903 100644
--- a/ArduCopter/baro_ground_effect.cpp
+++ b/ArduCopter/baro_ground_effect.cpp
@@ -23,7 +23,7 @@ void Copter::update_ground_effect_detector(void)
         xy_des_speed_cms = vel_target.length();
     }
 
-    if (position_ok() || optflow_position_ok()) {
+    if (position_ok() || ekf_has_relative_position()) {
         Vector3f vel = inertial_nav.get_velocity();
         vel.z = 0.0f;
         xy_speed_cms = vel.length();
@@ -52,7 +52,7 @@ void Copter::update_ground_effect_detector(void)
     // landing logic
     Vector3f angle_target_rad = attitude_control->get_att_target_euler_cd() * radians(0.01f);
     bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
-    bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
+    bool xy_speed_low = (position_ok() || ekf_has_relative_position()) && xy_speed_cms <= 125.0f;
     bool xy_speed_demand_low = pos_control->is_active_xy() && xy_des_speed_cms <= 125.0f;
     bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_xy()) || (control_mode == Mode::Number::ALT_HOLD && small_angle_request);
 
@@ -74,7 +74,7 @@ void Copter::update_ground_effect_detector(void)
 void Copter::update_ekf_terrain_height_stable()
 {
     // set to false if no position estimate
-    if (!position_ok() && !optflow_position_ok()) {
+    if (!position_ok() && !ekf_has_relative_position()) {
         ahrs.set_terrain_hgt_stable(false);
     }
 
diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp
index 4831b62f3f..fca5636e32 100644
--- a/ArduCopter/ekf_check.cpp
+++ b/ArduCopter/ekf_check.cpp
@@ -46,13 +46,15 @@ void Copter::ekf_check()
         return;
     }
 
-    // compare compass and velocity variance vs threshold
-    if (ekf_over_threshold()) {
+    // compare compass and velocity variance vs threshold and also check
+    // if we are still navigating
+    bool is_navigating = ekf_has_relative_position() || ekf_has_absolute_position();
+    if (ekf_over_threshold() || !is_navigating) {
         // if compass is not yet flagged as bad
         if (!ekf_check_state.bad_variance) {
             // increase counter
             ekf_check_state.fail_count++;
-            if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2)) {
+            if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2) && ekf_over_threshold()) {
                 // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset
                 // yaw to resolve the issue
                 ahrs.request_yaw_reset();
@@ -132,11 +134,7 @@ bool Copter::ekf_over_threshold()
         return true;
     }
 
-    // either optflow relative or absolute position estimate OK
-    if (optflow_position_ok() || ekf_position_ok()) {
-        return false;
-    }
-    return true;
+    return false;
 }
 
 
diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp
index bf74a983e9..d0884bb83c 100644
--- a/ArduCopter/system.cpp
+++ b/ArduCopter/system.cpp
@@ -325,11 +325,11 @@ bool Copter::position_ok() const
     }
 
     // check ekf position estimate
-    return (ekf_position_ok() || optflow_position_ok());
+    return (ekf_has_absolute_position() || ekf_has_relative_position());
 }
 
-// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
-bool Copter::ekf_position_ok() const
+// ekf_has_absolute_position - returns true if the EKF can provide an absolute WGS-84 position estimate
+bool Copter::ekf_has_absolute_position() const
 {
     if (!ahrs.have_inertial_nav()) {
         // do not allow navigation with dcm position
@@ -348,8 +348,8 @@ bool Copter::ekf_position_ok() const
     }
 }
 
-// optflow_position_ok - returns true if optical flow based position estimate is ok
-bool Copter::optflow_position_ok() const
+// ekf_has_relative_position - returns true if the EKF can provide a position estimate relative to it's starting position
+bool Copter::ekf_has_relative_position() const
 {
     // return immediately if EKF not used
     if (!ahrs.have_inertial_nav()) {