mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 01:53:34 +08:00
ekf2: only set fault status bad_acc_clipping if clipping frequently
This commit is contained in:
@@ -812,6 +812,8 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
_clip_counter--;
|
||||
}
|
||||
|
||||
_fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2;
|
||||
|
||||
const bool is_clipping_frequently = _clip_counter > 0;
|
||||
|
||||
// if vertical velocity and position are independent and agree, then do not require evidence of clipping if
|
||||
|
||||
@@ -237,24 +237,19 @@ void Ekf::predictCovariance()
|
||||
dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
|
||||
|
||||
// Accelerometer Clipping
|
||||
_fault_status.flags.bad_acc_clipping = false; // reset flag
|
||||
|
||||
// delta velocity X: increase process noise if sample contained any X axis clipping
|
||||
if (_imu_sample_delayed.delta_vel_clipping[0]) {
|
||||
dvxVar = sq(dt * BADACC_BIAS_PNOISE);
|
||||
_fault_status.flags.bad_acc_clipping = true;
|
||||
}
|
||||
|
||||
// delta velocity Y: increase process noise if sample contained any Y axis clipping
|
||||
if (_imu_sample_delayed.delta_vel_clipping[1]) {
|
||||
dvyVar = sq(dt * BADACC_BIAS_PNOISE);
|
||||
_fault_status.flags.bad_acc_clipping = true;
|
||||
}
|
||||
|
||||
// delta velocity Z: increase process noise if sample contained any Z axis clipping
|
||||
if (_imu_sample_delayed.delta_vel_clipping[2]) {
|
||||
dvzVar = sq(dt * BADACC_BIAS_PNOISE);
|
||||
_fault_status.flags.bad_acc_clipping = true;
|
||||
}
|
||||
|
||||
// predict the covariance
|
||||
|
||||
Reference in New Issue
Block a user