Move far-from-ekf-origin sanity check up to AP_Vehicle

ArduCopter/Copter.h

@@ -741,7 +741,6 @@ class Copter : public AP_Vehicle {
     void set_home_to_current_location_inflight();
     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
     bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
-    bool far_from_EKF_origin(const Location& loc);
 
     // compassmot.cpp
     MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan);

ArduCopter/commands.cpp

@@ -81,19 +81,3 @@ bool Copter::set_home(const Location& loc, bool lock)
     // return success
     return true;
 }
-
-// far_from_EKF_origin - checks if a location is too far from the EKF origin
-//  returns true if too far
-bool Copter::far_from_EKF_origin(const Location& loc)
-{
-    // check distance to EKF origin
-    Location ekf_origin;
-    if (ahrs.get_origin(ekf_origin)) {
-        if (labs(ekf_origin.alt - loc.alt)*0.01 > EKF_ORIGIN_MAX_ALT_KM*1000.0) {
-            return true;
-        }
-    }
-
-    // close enough to origin
-    return false;
-}

ArduCopter/config.h

@@ -130,10 +130,6 @@
  # define FS_EKF_THRESHOLD_DEFAULT      0.8f    // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
 #endif
 
-#ifndef EKF_ORIGIN_MAX_ALT_KM
- # define EKF_ORIGIN_MAX_ALT_KM         50   // EKF origin and home must be within 50km vertically
-#endif
-
 #ifndef FS_EKF_FILT_DEFAULT
 # define FS_EKF_FILT_DEFAULT     5.0f    // frequency cutoff of EKF variance filters
 #endif

ArduSub/Sub.h

@@ -425,7 +425,6 @@ class Sub : public AP_Vehicle {
     void set_home_to_current_location_inflight();
     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
     bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
-    bool far_from_EKF_origin(const Location& loc);
     float get_alt_rel() const WARN_IF_UNUSED;
     float get_alt_msl() const WARN_IF_UNUSED;
     void exit_mission();

ArduSub/commands.cpp

@@ -73,12 +73,3 @@ bool Sub::set_home(const Location& loc, bool lock)
     // return success
     return true;
 }
-
-// far_from_EKF_origin - checks if a location is too far from the EKF origin
-//  returns true if too far
-bool Sub::far_from_EKF_origin(const Location& loc)
-{
-    // check distance to EKF origin
-    Location ekf_origin;
-    return ahrs.get_origin(ekf_origin) && (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M);
-}

ArduSub/config.h

@@ -87,10 +87,6 @@
 # define MAV_SYSTEM_ID          1
 #endif
 
-#ifndef EKF_ORIGIN_MAX_DIST_M
-# define EKF_ORIGIN_MAX_DIST_M         50000   // EKF origin and waypoints (including home) must be within 50km
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // Nav-Guided - allows external nav computer to control vehicle
 #ifndef NAV_GUIDED

Blimp/Blimp.h

@@ -312,7 +312,6 @@ class Blimp : public AP_Vehicle
     void set_home_to_current_location_inflight();
     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
     bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
-    bool far_from_EKF_origin(const Location& loc);
 
     // ekf_check.cpp
     void ekf_check();

Blimp/commands.cpp

@@ -76,17 +76,3 @@ bool Blimp::set_home(const Location& loc, bool lock)
     // return success
     return true;
 }
-
-// far_from_EKF_origin - checks if a location is too far from the EKF origin
-//  returns true if too far
-bool Blimp::far_from_EKF_origin(const Location& loc)
-{
-    // check distance to EKF origin
-    Location ekf_origin;
-    if (ahrs.get_origin(ekf_origin) && ((ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M) || (labs(ekf_origin.alt - loc.alt) > EKF_ORIGIN_MAX_DIST_M))) {
-        return true;
-    }
-
-    // close enough to origin
-    return false;
-}

Blimp/config.h

@@ -67,10 +67,6 @@
 # define FS_EKF_THRESHOLD_DEFAULT      0.8f    // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
 #endif
 
-#ifndef EKF_ORIGIN_MAX_DIST_M
-# define EKF_ORIGIN_MAX_DIST_M         50000   // EKF origin and waypoints (including home) must be within 50km
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // FLIGHT_MODE
 //

Tools/autotest/blimp.py

@@ -254,6 +254,7 @@ def tests(self):
             self.FlyManual,
             self.FlyLoiter,
             self.PREFLIGHT_Pressure,
+            self.far_from_EKF_origin,
         ])
         return ret
 

Tools/autotest/vehicle_test_suite.py

@@ -13554,6 +13554,18 @@ def MSP_DJI(self):
             if dist < 1:
                 break
 
+    def far_from_EKF_origin(self):
+        '''test that we get a failure if home is set far from the EKF origin'''
+        self.wait_ready_to_arm()
+        here = self.poll_home_position()
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_DO_SET_HOME,
+            p5=here.latitude,
+            p6=here.longitude,
+            p7=6000000,
+            want_result=mavutil.mavlink.MAV_RESULT_DENIED,
+        )
+
     def CRSF(self):
         '''Test RC CRSF'''
         self.context_push()

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -1098,6 +1098,28 @@ bool AP_Vehicle::block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_lis
 }
 #endif
 
+#if AP_AHRS_ENABLED
+#ifndef EKF_ORIGIN_MAX_ALT_KM
+#define EKF_ORIGIN_MAX_ALT_KM         50   // EKF origin and home must be within 50km vertically
+#endif
+
+// far_from_EKF_origin - checks if a location is too far from the EKF origin
+//  returns true if too far
+bool AP_Vehicle::far_from_EKF_origin(const Location& loc)
+{
+    // check distance to EKF origin
+    Location ekf_origin;
+    if (ahrs.get_origin(ekf_origin)) {
+        if (labs(ekf_origin.alt - loc.alt)*0.01 > EKF_ORIGIN_MAX_ALT_KM*1000.0) {
+            return true;
+        }
+    }
+
+    // close enough to origin
+    return false;
+}
+#endif  // AP_AHRS_ENABLED
+
 AP_Vehicle *AP_Vehicle::_singleton = nullptr;
 
 AP_Vehicle *AP_Vehicle::get_singleton()

libraries/AP_Vehicle/AP_Vehicle.h

@@ -493,6 +493,12 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     // Check if this mode can be entered from the GCS
     bool block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const;
 
+#if AP_AHRS_ENABLED
+// far_from_EKF_origin - checks if a location is too far from the EKF origin
+//  returns true if too far
+    bool far_from_EKF_origin(const Location& loc);
+#endif
+
 private:
 
     // delay() callback that processing MAVLink packets