mirror of https://github.com/ArduPilot/ardupilot
Copter: make EKF mag variance check use max
this makes it consistent with logged value
This commit is contained in:
parent
4f7450dde9
commit
768dcb6768
|
@ -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))) {
|
||||||
|
|
Loading…
Reference in New Issue