Copter: factor out a ekf_check_position_problem method

ArduCopter/Copter.h

@@ -972,6 +972,7 @@ private:
 
     void ekf_check();
     bool ekf_over_threshold();
+    bool ekf_check_position_problem();
     void failsafe_ekf_event();
     void failsafe_ekf_off_event(void);
     void esc_calibration_startup_check();

ArduCopter/ekf_check.cpp

@@ -44,7 +44,7 @@ void Copter::ekf_check()
     }
 
     // compare compass and velocity variance vs threshold
-    if (ekf_over_threshold()) {
+    if (ekf_over_threshold() || ekf_check_position_problem()) {
         // if compass is not yet flagged as bad
         if (!ekf_check_state.bad_variance) {
             // increase counter
@@ -86,6 +86,21 @@ void Copter::ekf_check()
     // To-Do: add ekf variances to extended status
 }
 
+// ekf_check_position_problem - returns true if the EKF has a positioning problem
+bool Copter::ekf_check_position_problem()
+{
+    // either otflow or abs means we're OK:
+    if (optflow_position_ok()) {
+        return false;
+    }
+    if (ekf_position_ok()) {
+        return false;
+    }
+
+    return true;
+}
+
+
 // ekf_over_threshold - returns true if the ekf's variance are over the tolerance
 bool Copter::ekf_over_threshold()
 {
@@ -94,11 +109,6 @@ bool Copter::ekf_over_threshold()
         return false;
     }
 
-    // return true immediately if position is bad
-    if (!ekf_position_ok() && !optflow_position_ok()) {
-        return true;
-    }
-
     // use EKF to get variance
     float position_variance, vel_variance, height_variance, tas_variance;
     Vector3f mag_variance;