Browse Source

ekf2: only set fault status bad_acc_clipping if clipping frequently

master
Daniel Agar 3 years ago
parent
commit
49d9d1c987
  1. 2
      src/modules/ekf2/EKF/control.cpp
  2. 5
      src/modules/ekf2/EKF/covariance.cpp

2
src/modules/ekf2/EKF/control.cpp

@ -812,6 +812,8 @@ void Ekf::checkVerticalAccelerationHealth() @@ -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

5
src/modules/ekf2/EKF/covariance.cpp

@ -237,24 +237,19 @@ void Ekf::predictCovariance() @@ -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

Loading…
Cancel
Save