Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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; float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance; Vector3f mag_variance;
ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_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"); check_failed(display_failure, "EKF compass variance");
return false; 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 // check home and EKF origin are not too far
if (copter.far_from_EKF_origin(ahrs.get_home())) { if (copter.far_from_EKF_origin(ahrs.get_home())) {