Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: improve check of far_from_EKF_origin 

- increases the maximum distance from EKF origin to 250km horizontally
- adds arming check that the vehicle is within 250km of the EKF origin
- fixes a bug in the far-from-EKF-origin related to height (was 500m, now 50km)
This commit is contained in:
Andrew Tridgell 2021-06-28 11:05:19 +10:00 committed by Randy Mackay
parent 4c7a9146c6
commit 5eb1d4a5a9
3 changed files with 20 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/AP_Arming.cpp
View File

@ -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;
}

9
ArduCopter/commands.cpp
View File

@ -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

7
ArduCopter/config.h
View File

@ -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