Copter: replace GPS_ok with position_ok

position_ok uses the EKF's filter status if the EKF is being used
otherwise it falls back to the GPS based checks used by inertial nav

ArduCopter/control_auto.pde

@@ -20,7 +20,7 @@
 // auto_init - initialise auto controller
 static bool auto_init(bool ignore_checks)
 {
-    if ((GPS_ok() && inertial_nav.get_filter_status().flags.horiz_pos_abs && mission.num_commands() > 1) || ignore_checks) {
+    if ((position_ok() && inertial_nav.get_filter_status().flags.horiz_pos_abs && mission.num_commands() > 1) || ignore_checks) {
         auto_mode = Auto_Loiter;
 
         // stop ROI from carrying over from previous runs of the mission
@@ -430,7 +430,7 @@ void auto_nav_guided_run()
 bool auto_loiter_start()
 {
     // return failure if GPS is bad
-    if (!GPS_ok()) {
+    if (!position_ok()) {
         return false;
     }
     auto_mode = Auto_Loiter;

ArduCopter/control_circle.pde

@@ -7,7 +7,7 @@
 // circle_init - initialise circle controller flight mode
 static bool circle_init(bool ignore_checks)
 {
-    if ((GPS_ok() && inertial_nav.get_filter_status().flags.horiz_pos_abs) || ignore_checks) {
+    if ((position_ok() && inertial_nav.get_filter_status().flags.horiz_pos_abs) || ignore_checks) {
         circle_pilot_yaw_override = false;
 
         // initialize speeds and accelerations

ArduCopter/control_drift.pde

@@ -29,7 +29,7 @@
 // drift_init - initialise drift controller
 static bool drift_init(bool ignore_checks)
 {
-    if (GPS_ok() || ignore_checks) {
+    if (position_ok() || ignore_checks) {
         return true;
     }else{
         return false;

ArduCopter/control_guided.pde

@@ -26,7 +26,7 @@ struct Guided_Limit {
 // guided_init - initialise guided controller
 static bool guided_init(bool ignore_checks)
 {
-    if (GPS_ok() || ignore_checks) {
+    if (position_ok() || ignore_checks) {
         // initialise yaw
         set_auto_yaw_mode(get_default_auto_yaw_mode(false));
         // start in position control mode

ArduCopter/control_land.pde

@@ -9,7 +9,7 @@ static bool land_pause;
 static bool land_init(bool ignore_checks)
 {
     // check if we have GPS and decide which LAND we're going to do
-    land_with_gps = GPS_ok();
+    land_with_gps = position_ok();
     if (land_with_gps) {
         // set target to stopping point
         Vector3f stopping_point;

ArduCopter/control_loiter.pde

@@ -7,7 +7,7 @@
 // loiter_init - initialise loiter controller
 static bool loiter_init(bool ignore_checks)
 {
-    if (GPS_ok() || ignore_checks) {
+    if (position_ok() || ignore_checks) {
 
         // set target to current position
         wp_nav.init_loiter_target();

ArduCopter/control_poshold.pde

@@ -97,7 +97,7 @@ static struct {
 static bool poshold_init(bool ignore_checks)
 {
     // fail to initialise PosHold mode if no GPS lock
-    if (!GPS_ok() && !ignore_checks) {
+    if (!position_ok() && !ignore_checks) {
         return false;
     }
     

ArduCopter/control_rtl.pde

@@ -10,7 +10,7 @@
 // rtl_init - initialise rtl controller
 static bool rtl_init(bool ignore_checks)
 {
-    if (GPS_ok() || ignore_checks) {
+    if (position_ok() || ignore_checks) {
         rtl_climb_start();
         return true;
     }else{

ArduCopter/heli.pde

@@ -51,7 +51,7 @@ static void check_dynamic_flight(void)
     bool moving = false;
 
     // with GPS lock use inertial nav to determine if we are moving
-    if (GPS_ok()) {
+    if (position_ok()) {
         // get horizontal velocity
         float velocity = inertial_nav.get_velocity_xy();
         moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);

ArduCopter/motors.pde

@@ -508,7 +508,7 @@ static bool pre_arm_gps_checks(bool display_failure)
     }
 
     // ensure GPS is ok
-    if (!GPS_ok()) {
+    if (!position_ok()) {
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
         }

ArduCopter/navigation.pde

@@ -64,7 +64,7 @@ static void calc_home_distance_and_bearing()
     Vector3f curr = inertial_nav.get_position();
 
     // calculate home distance and bearing
-    if (GPS_ok()) {
+    if (position_ok()) {
         home_distance = pythagorous2(curr.x, curr.y);
         home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));
 

ArduCopter/system.pde

@@ -335,15 +335,17 @@ static void startup_ground(bool force_gyro_cal)
     set_land_complete_maybe(true);
 }
 
-// returns true if the GPS is ok and home position is set
-static bool GPS_ok()
+// position_ok - returns true if the horizontal absolute position is ok and home position is set
+static bool position_ok()
 {
-    if (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
-        !gps_glitch.glitching() && !failsafe.gps &&
-        !ekf_check_state.bad_compass && !failsafe.ekf) {
-        return true;
-    }else{
-        return false;
+    if (ahrs.have_inertial_nav()) {
+        // with EKF use filter status and ekf check
+        return (inertial_nav.get_filter_status().flags.horiz_pos_abs && !failsafe.ekf);
+    } else {
+        // with interial nav use GPS based checks
+        return (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
+                !gps_glitch.glitching() && !failsafe.gps &&
+                !ekf_check_state.bad_compass && !failsafe.ekf);
     }
 }