|
|
|
@ -100,13 +100,13 @@ bool Copter::ekf_over_threshold()
@@ -100,13 +100,13 @@ bool Copter::ekf_over_threshold()
|
|
|
|
|
Vector2f offset; |
|
|
|
|
ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset); |
|
|
|
|
|
|
|
|
|
// return true if two of compass, velocity and position variances are over 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; |
|
|
|
|
if (mag_variance.length() >= g.fs_ekf_thresh) { |
|
|
|
|
over_thresh_count++; |
|
|
|
|
} |
|
|
|
|
if (vel_variance >= (2.0f * g.fs_ekf_thresh)) { |
|
|
|
|
over_thresh_count+=2; |
|
|
|
|
over_thresh_count += 2; |
|
|
|
|
} else if (vel_variance >= g.fs_ekf_thresh) { |
|
|
|
|
over_thresh_count++; |
|
|
|
|
} |
|
|
|
|