dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Make clip detection logic easier to follow

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
EKF/control.cpp
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; const uint16_t clip_count_limit = 1000 / FILTER_UPDATE_PERIOD_MS;
if (_imu_sample_delayed.delta_vel_clipping[0] || const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] ||
_imu_sample_delayed.delta_vel_clipping[1] || _imu_sample_delayed.delta_vel_clipping[1] ||
_imu_sample_delayed.delta_vel_clipping[2]) { _imu_sample_delayed.delta_vel_clipping[2];
if (_clip_counter < clip_count_limit) { if (is_clipping &&_clip_counter < clip_count_limit) {
_clip_counter++; _clip_counter++;
}
} else if (_clip_counter > 0) { } else if (_clip_counter > 0) {
_clip_counter--; _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 // if vertical velocity and position are independent and agree, then do not require evidence of clipping if
// innovations are large // 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; is_inertial_nav_falling;
if (bad_vert_accel) { if (bad_vert_accel) {