From 3443e1c12f1c1777634eae0f1a5714b4d4265428 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 9 Apr 2024 17:41:12 +1000 Subject: [PATCH] ArduSub: remove far_from_EKF_origin sanity checks some flawed implementations, and the extreme-ardupilot project means these checks are no longer required --- ArduSub/Sub.h | 1 - ArduSub/commands.cpp | 9 --------- ArduSub/commands_logic.cpp | 6 ++---- 3 files changed, 2 insertions(+), 14 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 98d36db4c2..6aba8c7186 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -425,7 +425,6 @@ private: 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(); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index 7009f8cd21..b12626ed4e 100644 --- a/ArduSub/commands.cpp +++ b/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); -} diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 392749c6cf..a77f4111e7 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -679,10 +679,8 @@ void Sub::do_set_home(const AP_Mission::Mission_Command& cmd) // silently ignore this failure } } else { - if (!far_from_EKF_origin(cmd.content.location)) { - if (!set_home(cmd.content.location, false)) { - // silently ignore this failure - } + if (!set_home(cmd.content.location, false)) { + // silently ignore this failure } } }