From 2f71a40fc2d6d3e31bc9bc87d4ba1ff99df6b069 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 9 Apr 2024 17:41:12 +1000 Subject: [PATCH 1/3] ArduCopter: remove far_from_EKF_origin sanity checks some flawed implementations, and the extreme-ardupilot project means these checks are no longer required --- ArduCopter/AP_Arming.cpp | 12 ------------ ArduCopter/Copter.h | 1 - ArduCopter/commands.cpp | 21 --------------------- 3 files changed, 34 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index cfc5ebc2d351f..4fbfe125ccd30 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -495,18 +495,6 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) } } - // check if home is too far from EKF origin - if (copter.far_from_EKF_origin(ahrs.get_home())) { - check_failed(display_failure, "Home too far from EKF origin"); - return false; - } - - // check if vehicle is too far from EKF origin - if (copter.far_from_EKF_origin(copter.current_loc)) { - check_failed(display_failure, "Vehicle too far from EKF origin"); - return false; - } - // if we got here all must be ok return true; } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index e0e891432efee..3285f267976e5 100644 --- a/ArduCopter/Copter.h +++ b/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); diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index e5c2f152d03b1..fea41d19e5563 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -63,11 +63,6 @@ bool Copter::set_home(const Location& loc, bool lock) return false; } - // check home is close to EKF origin - if (far_from_EKF_origin(loc)) { - return false; - } - // set ahrs home (used for RTL) if (!ahrs.set_home(loc)) { return false; @@ -81,19 +76,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; -} From 6b90dc5a7271cbbebe7eba186f22398f6ee6c22a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 9 Apr 2024 17:41:12 +1000 Subject: [PATCH 2/3] 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 98d36db4c2d93..6aba8c71861ef 100644 --- a/ArduSub/Sub.h +++ b/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(); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index 7009f8cd21cf8..b12626ed4e806 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 392749c6cf301..a77f4111e7254 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 } } } From 8ff846ded531659767c45b7ce53eb61771fe1599 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 9 Apr 2024 17:41:12 +1000 Subject: [PATCH 3/3] Blimp: remove far_from_EKF_origin sanity checks some flawed implementations, and the extreme-ardupilot project means these checks are no longer required --- Blimp/Blimp.h | 1 - Blimp/commands.cpp | 19 ------------------- 2 files changed, 20 deletions(-) diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index ccaa200a1c989..086f17f44a552 100644 --- a/Blimp/Blimp.h +++ b/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(); diff --git a/Blimp/commands.cpp b/Blimp/commands.cpp index 28942deb39315..2185955e2454f 100644 --- a/Blimp/commands.cpp +++ b/Blimp/commands.cpp @@ -58,11 +58,6 @@ bool Blimp::set_home(const Location& loc, bool lock) return false; } - // check home is close to EKF origin - if (far_from_EKF_origin(loc)) { - return false; - } - // set ahrs home (used for RTL) if (!ahrs.set_home(loc)) { return false; @@ -76,17 +71,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; -}