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

Copter: pre-arm check of EKF pos and vel variances

This commit is contained in:
Randy Mackay 2021-03-10 14:26:16 +09:00
parent 49486b7cea
commit e0a8baccfd
1 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/AP_Arming.cpp
View File

@ -554,14 +554,24 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
}
}
// check EKF compass variance is below failsafe threshold
// check EKF's compass, position and velocity variances are below failsafe threshold
if (copter.g.fs_ekf_thresh > 0.0f) {
float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;
ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance);
if (copter.g.fs_ekf_thresh > 0 && mag_variance.length() >= copter.g.fs_ekf_thresh) {
if (mag_variance.length() >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF compass variance");
return false;
}
if (pos_variance >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF position variance");
return false;
}
if (vel_variance >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF velocity variance");
return false;
}
}
// check home and EKF origin are not too far
if (copter.far_from_EKF_origin(ahrs.get_home())) {