Rover: allow vehicle to be armed in manual with no GPS

APMrover2/AP_Arming.cpp

@@ -57,3 +57,15 @@ bool AP_Arming_Rover::pre_arm_rc_checks(const bool display_failure)
     }
     return true;
 }
+
+// performs pre_arm gps related checks and returns true if passed
+bool AP_Arming_Rover::gps_checks(bool display_failure)
+{
+    if (!rover.control_mode->requires_gps()) {
+        // we don't care!
+        return true;
+    }
+
+    // call parent gps checks
+    return AP_Arming::gps_checks(display_failure);
+}

APMrover2/AP_Arming.h

@@ -19,6 +19,7 @@ public:
     AP_Arming_Rover &operator=(const AP_Baro&) = delete;
 
     bool pre_arm_rc_checks(const bool display_failure);
+    bool gps_checks(bool display_failure) override;
 
 protected:
     AP_Arming_Rover(const AP_AHRS &ahrs_ref, const AP_Baro &baro, Compass &compass,

APMrover2/mode.cpp

@@ -1,6 +1,8 @@
 #include "mode.h"
 #include "Rover.h"
 
+#define MODE_AHRS_GPS_ERROR_MAX    10      // accept up to 10m difference between AHRS and GPS
+
 Mode::Mode() :
     ahrs(rover.ahrs),
     g(rover.g),
@@ -19,8 +21,56 @@ void Mode::exit()
     lateral_acceleration = 0.0f;
 }
 
+// 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");
+    }
+
+    // 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()) {
+            return false;
+        }
+    }
+
     return _enter();
 }
 

APMrover2/mode.h

@@ -62,6 +62,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; }
+
     //
     // navigation methods
     //
@@ -145,6 +148,9 @@ 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;
+
 };
 
 
@@ -301,6 +307,9 @@ public:
     // attributes for mavlink system status reporting
     bool has_manual_input() const override { return true; }
     bool attitude_stabilized() const override { return false; }
+
+    bool requires_gps() const override { return false; }
+
 };