From 9cba1e290716994b57762eb4cbf363480f59ada6 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 22 Jan 2018 20:00:47 +0900
Subject: [PATCH] Rover: gps check replaced by requires-position,
 requires-velocity

requires_gps is replaced by requires_position and requires_velocity
enter_gps_checks method is removed and enter method directly checks ekf flags and ekf origin
---
 APMrover2/mode.cpp | 59 +++++++++++++---------------------------------
 APMrover2/mode.h   | 26 +++++++++++++-------
 2 files changed, 33 insertions(+), 52 deletions(-)

diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp
index 794bb67302..4bb452e362 100644
--- a/APMrover2/mode.cpp
+++ b/APMrover2/mode.cpp
@@ -19,53 +19,26 @@ void Mode::exit()
     _exit();
 }
 
-// these are basically the same checks as in AP_Arming:
-bool Mode::enter_gps_checks() const
-{
-    const AP_GPS &gps = AP::gps();
-
-    if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
-        gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad GPS Position");
-        return false;
-    }
-    //GPS update rate acceptable
-    if (!gps.is_healthy()) {
-        gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS is not healthy");
-        return false;
-    }
-
-    // check GPSs are within 50m of each other and that blending is healthy
-    float distance_m;
-    if (!gps.all_consistent(distance_m)) {
-        gcs().send_text(MAV_SEVERITY_CRITICAL,
-                        "GPS positions differ by %4.1fm",
-                        (double)distance_m);
-        return false;
-    }
-    if (!gps.blend_health_check()) {
-        gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS blending unhealthy");
-        return false;
-    }
-
-    // check AHRS and GPS are within 10m of each other
-    const Location gps_loc = gps.location();
-    Location ahrs_loc;
-    if (ahrs.get_position(ahrs_loc)) {
-        float distance = location_3d_diff_NED(gps_loc, ahrs_loc).length();
-        if (distance > MODE_AHRS_GPS_ERROR_MAX) {
-            gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS and AHRS differ by %4.1fm", (double)distance);
-            return false;
-        }
-    }
-
-    return true;
-}
-
 bool Mode::enter()
 {
     const bool ignore_checks = !hal.util->get_soft_armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform
     if (!ignore_checks) {
-        if (requires_gps() && !enter_gps_checks()) {
+
+        // get EKF filter status
+        nav_filter_status filt_status;
+        rover.ahrs.get_filter_status(filt_status);
+
+        // check position estimate.  requires origin and at least one horizontal position flag to be true
+        Location origin;
+        const bool position_ok = ahrs.get_origin(origin) &&
+                                (filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs ||
+                                 filt_status.flags.horiz_pos_rel || filt_status.flags.pred_horiz_pos_rel);
+        if (requires_position() && !position_ok) {
+            return false;
+        }
+
+        // check velocity estimate (if we have position estimate, we must have velocity estimate)
+        if (requires_velocity() && !position_ok && !filt_status.flags.horiz_vel) {
             return false;
         }
     }
diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index e42f7bed7f..a91b1f8cd2 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -55,8 +55,9 @@ public:
     // true if heading is controlled
     virtual bool attitude_stabilized() const { return true; }
 
-    // true if mode requires GPS:
-    virtual bool requires_gps() const { return true; }
+    // true if mode requires position and/or velocity estimate
+    virtual bool requires_position() const { return true; }
+    virtual bool requires_velocity() const { return true; }
 
     //
     // navigation methods
@@ -155,9 +156,6 @@ protected:
     float _desired_speed;       // desired speed in m/s
     float _desired_speed_final; // desired speed in m/s when we reach the destination
     float _speed_error;         // ground speed error in m/s
-
-    bool enter_gps_checks() const;
-
 };
 
 
@@ -173,6 +171,10 @@ public:
 
     // attributes for mavlink system status reporting
     bool has_manual_input() const override { return true; }
+
+    // acro mode requires a velocity estimate
+    bool requires_position() const override { return false; }
+    bool requires_velocity() const override { return true; }
 };
 
 
@@ -292,8 +294,9 @@ public:
     // attributes for mavlink system status reporting
     bool attitude_stabilized() const override { return false; }
 
-    // hold mode does not require GPS
-    bool requires_gps() const override { return false; }
+    // hold mode does not require position or velocity estimate
+    bool requires_position() const override { return false; }
+    bool requires_velocity() const override { return false; }
 };
 
 
@@ -311,8 +314,9 @@ public:
     bool has_manual_input() const override { return true; }
     bool attitude_stabilized() const override { return false; }
 
-    // manual mode does not require GPS
-    bool requires_gps() const override { return false; }
+    // manual mode does not require position or velocity estimate
+    bool requires_position() const override { return false; }
+    bool requires_velocity() const override { return false; }
 };
 
 
@@ -383,6 +387,10 @@ public:
 
     // attributes for mavlink system status reporting
     bool has_manual_input() const override { return true; }
+
+    // steering requires velocity but not position
+    bool requires_position() const override { return false; }
+    bool requires_velocity() const override { return true; }
 };
 
 class ModeInitializing : public Mode