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

Copter: make EKF mag variance check use max 

this makes it consistent with logged value
This commit is contained in:
Andrew Tridgell 2020-04-04 16:37:36 +11:00 committed by Randy Mackay
parent 8be1755b12
commit 95f896b556
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,9 +105,11 @@ bool Copter::ekf_over_threshold()
Vector2f offset; Vector2f offset;
ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset); ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset);
const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);
// return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold
uint8_t over_thresh_count = 0; uint8_t over_thresh_count = 0;
if (mag_variance.length() >= g.fs_ekf_thresh) { if (mag_max >= g.fs_ekf_thresh) {
over_thresh_count++; over_thresh_count++;
} }
if (!optflow.healthy() && (vel_variance >= (2.0f * g.fs_ekf_thresh))) { if (!optflow.healthy() && (vel_variance >= (2.0f * g.fs_ekf_thresh))) {