Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Arming: check EKF height varance

Browse Source
This commit is contained in:
Iampete1 2022-08-26 11:43:59 +01:00 committed by Randy Mackay
parent 2bcb503170
commit 353aedb547
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/AP_Arming.cpp
View File

@ -498,7 +498,7 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
} }
} }
// check EKF's compass, position and velocity variances are below failsafe threshold // check EKF's compass, position, height and velocity variances are below failsafe threshold
if (copter.g.fs_ekf_thresh > 0.0f) { 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;
@ -515,6 +515,10 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
check_failed(display_failure, "EKF velocity variance"); check_failed(display_failure, "EKF velocity variance");
return false; return false;
} }
if (hgt_variance >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF height variance");
return false;
}
} }
// check if home is too far from EKF origin // check if home is too far from EKF origin

2
ArduCopter/Parameters.cpp
View File

@ -404,7 +404,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FS_EKF_THRESH // @Param: FS_EKF_THRESH
// @DisplayName: EKF failsafe variance threshold // @DisplayName: EKF failsafe variance threshold
// @Description: Allows setting the maximum acceptable compass and velocity variance // @Description: Allows setting the maximum acceptable compass, velocity, position and height variances. Used in arming check and EKF failsafe.
// @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed
// @User: Advanced // @User: Advanced
GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT), GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT),