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

Copter: speed up EKF failsafe by checking if velocity innovations > 2x FS_EKF_THRESH

This commit is contained in:
chobits 2019-03-13 16:21:14 +08:00 committed by Randy Mackay
parent c40312d0f9
commit 6bee4216c0
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ekf_check.cpp
View File

@ -105,7 +105,9 @@ bool Copter::ekf_over_threshold()
if (mag_variance.length() >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (vel_variance >= g.fs_ekf_thresh) {
if (vel_variance >= (2.0f * g.fs_ekf_thresh)) {
over_thresh_count+=2;
} else if (vel_variance >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (position_variance >= g.fs_ekf_thresh) {