diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index eb7cb5384e..228d3b895b 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -573,9 +573,15 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) } } - // check home and EKF origin are not too far + // check if home is too far from EKF origin if (copter.far_from_EKF_origin(ahrs.get_home())) { - check_failed(display_failure, "EKF-home variance"); + 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; } diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index 0ceb2a73d2..54b0896128 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -107,8 +107,13 @@ bool Copter::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; + 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; + } } // close enough to origin diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 5cbe6c15c6..c906530437 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -168,8 +168,11 @@ # 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 +#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 #ifndef COMPASS_CAL_STICK_GESTURE_TIME