From bd84ed126e64583de0552c2812a2fe2444bf9865 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Jul 2021 16:50:53 +1000 Subject: [PATCH] Copter: removed max home distance after discussion with Randy we agreed to just remove this check now we are numerically stable at long distances --- ArduCopter/commands.cpp | 3 --- ArduCopter/config.h | 3 --- 2 files changed, 6 deletions(-) diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index 54b0896128..fd0c22a630 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -108,9 +108,6 @@ bool Copter::far_from_EKF_origin(const Location& loc) // check distance to EKF origin Location ekf_origin; if (ahrs.get_origin(ekf_origin)) { - if ((ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_KM*1000.0)) { - return true; - } if (labs(ekf_origin.alt - loc.alt)*0.01 > EKF_ORIGIN_MAX_ALT_KM*1000.0) { return true; } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index d89ef3f8bc..757de1c8d1 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -168,9 +168,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_KM - # define EKF_ORIGIN_MAX_DIST_KM 250 // EKF origin and home must be within 250km horizontally -#endif #ifndef EKF_ORIGIN_MAX_ALT_KM # define EKF_ORIGIN_MAX_ALT_KM 50 // EKF origin and home must be within 50km vertically #endif