Rover: arming and mode init checks use ekf_position_ok

APMrover2/AP_Arming.cpp

@@ -55,6 +55,16 @@ bool AP_Arming_Rover::gps_checks(bool display_failure)
         return false;
     }
 
+    // ensure position esetimate is ok
+    if (!rover.ekf_position_ok()) {
+        const char *reason = AP::ahrs().prearm_failure_reason();
+        if (reason == nullptr) {
+            reason = "Need Position Estimate";
+        }
+        check_failed(ARMING_CHECK_NONE, display_failure, "%s", reason);
+        return false;
+    }
+
     // call parent gps checks
     return AP_Arming::gps_checks(display_failure);
 }

APMrover2/GCS_Mavlink.cpp

@@ -62,7 +62,7 @@ uint32_t GCS_MAVLINK_Rover::custom_mode() const
 
 MAV_STATE GCS_MAVLINK_Rover::system_status() const
 {
-    if (rover.failsafe.triggered != 0) {
+    if ((rover.failsafe.triggered != 0) || rover.failsafe.ekf) {
         return MAV_STATE_CRITICAL;
     }
     if (rover.control_mode == &rover.mode_initializing) {

APMrover2/defines.h

@@ -121,6 +121,7 @@ enum mode_reason_t {
     MODE_REASON_CRASH_FAILSAFE,
     MODE_REASON_MISSION_COMMAND,
     MODE_REASON_FENCE_BREACH,
+    MODE_REASON_EKF_FAILSAFE,
 };
 
 enum pilot_steer_type_t {

APMrover2/mode.cpp

@@ -27,10 +27,7 @@ bool Mode::enter()
         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);
+        const bool position_ok = rover.ekf_position_ok() && !rover.failsafe.ekf;
         if (requires_position() && !position_ok) {
             return false;
         }