Copter: fix ekf-check reliance on position_ok

ArduCopter/Copter.h

@@ -876,6 +876,7 @@ private:
     void init_ardupilot();
     void startup_ground(bool force_gyro_cal);
     bool position_ok();
+    bool ekf_position_ok();
     bool optflow_position_ok();
     void update_auto_armed();
     void check_usb_mux(void);

ArduCopter/ekf_check.cpp

@@ -91,7 +91,7 @@ bool Copter::ekf_over_threshold()
     }
 
     // return true immediately if position is bad
-    if (!position_ok() && !optflow_position_ok()) {
+    if (!ekf_position_ok() && !optflow_position_ok()) {
         return true;
     }
 

ArduCopter/system.cpp

@@ -309,13 +309,20 @@ void Copter::startup_ground(bool force_gyro_cal)
 // position_ok - returns true if the horizontal absolute position is ok and home position is set
 bool Copter::position_ok()
 {
-    if (!ahrs.have_inertial_nav()) {
-        // do not allow navigation with dcm position
+    // return false if ekf failsafe has triggered
+    if (failsafe.ekf) {
         return false;
     }
 
-    // return false if ekf failsafe has triggered
-    if (failsafe.ekf) {
+    // check ekf position estimate
+    return ekf_position_ok();
+}
+
+// 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()
+{
+    if (!ahrs.have_inertial_nav()) {
+        // do not allow navigation with dcm position
         return false;
     }