Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

EKF: Make clip detection logic easier to follow

Browse Source
This commit is contained in:
Paul Riseborough
2020-06-30 19:11:58 +10:00
committed by Paul Riseborough
parent 9aa8d93efc
commit b87778e61d
1 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+7 -7
View File
@@ -913,21 +913,21 @@ void Ekf::checkVerticalAccelerationHealth()
}
}
// Check for > 50% clipping affected IMU samples within the past 1 second
// Check for more than 50% clipping affected IMU samples within the past 1 second
const uint16_t clip_count_limit = 1000 / FILTER_UPDATE_PERIOD_MS;
if (_imu_sample_delayed.delta_vel_clipping[0] ||
_imu_sample_delayed.delta_vel_clipping[1] ||
_imu_sample_delayed.delta_vel_clipping[2]) {
if (_clip_counter < clip_count_limit) {
const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] ||
_imu_sample_delayed.delta_vel_clipping[1] ||
_imu_sample_delayed.delta_vel_clipping[2];
if (is_clipping &&_clip_counter < clip_count_limit) {
_clip_counter++;
}
} else if (_clip_counter > 0) {
_clip_counter--;
}
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
// innovations are large
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || _clip_counter > 0) &&
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) &&
is_inertial_nav_falling;
if (bad_vert_accel) {