fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 13:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: refactor vertical acceleration check for multiple height sources

Browse Source
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent 8fd79688c0
commit f9188b2a14
7 changed files with 230 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
-95
View File
@@ -657,101 +657,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
}
}
void Ekf::checkVerticalAccelerationHealth()
{
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
// innovations being positive and not stale.
// Clipping usually causes the average accel reading to move towards zero which makes the INS
// think it is falling and produces positive vertical innovations.
// Don't use stale innovation data.
bool is_inertial_nav_falling = false;
bool are_vertical_pos_and_vel_independant = false;
bool is_using_multiple_hgt_sources = false;
if (_control_status.flags.gps) {
// GNSS velocity
const auto &gps_vel = _aid_src_gnss_vel;
if (gps_vel.time_last_fuse[2] > _vert_vel_fuse_time_us) {
_vert_vel_fuse_time_us = gps_vel.time_last_fuse[2];
_vert_vel_innov_ratio = gps_vel.innovation[2] / sqrtf(gps_vel.innovation_variance[2]);
}
// GNSS position
const auto &gps_pos = _aid_src_gnss_pos;
if (gps_pos.time_last_fuse[2] > _vert_pos_fuse_attempt_time_us) {
_vert_pos_fuse_attempt_time_us = gps_pos.time_last_fuse[2];
_vert_pos_innov_ratio = gps_pos.innovation[2] / sqrtf(gps_pos.innovation_variance[2]);
}
}
if (_control_status.flags.baro_hgt) {
// baro height
const auto &baro_hgt = _aid_src_baro_hgt;
if (baro_hgt.time_last_fuse > _vert_pos_fuse_attempt_time_us) {
_vert_pos_fuse_attempt_time_us = baro_hgt.time_last_fuse;
_vert_pos_innov_ratio = baro_hgt.innovation / sqrtf(baro_hgt.innovation_variance);
}
}
if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
is_using_multiple_hgt_sources = _control_status.flags.gps_hgt && _control_status.flags.baro_hgt;
if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
// If vertical position and velocity come from independent sensors then we can
// trust them more if they disagree with the IMU, but need to check that they agree
const bool using_gps_for_both = _control_status.flags.gps_hgt && _control_status.flags.gps;
const bool using_ev_for_both = _control_status.flags.ev_hgt && _control_status.flags.ev_vel;
are_vertical_pos_and_vel_independant = !(using_gps_for_both || using_ev_for_both);
is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > _params.vert_innov_test_min;
is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > _params.vert_innov_test_min;
} else {
// only height sensing available
is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim;
}
}
// Check for more than 50% clipping affected IMU samples within the past 1 second
const uint16_t clip_count_limit = 1.f / _dt_ekf_avg;
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--;
}
_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
// innovations are large
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_using_multiple_hgt_sources || is_clipping_frequently) && is_inertial_nav_falling;
if (bad_vert_accel) {
_time_bad_vert_accel = _time_last_imu;
} else {
_time_good_vert_accel = _time_last_imu;
}
// declare a bad vertical acceleration measurement and make the declaration persist
// for a minimum of BADACC_PROBATION seconds
if (_fault_status.flags.bad_acc_vertical) {
_fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
} else {
_fault_status.flags.bad_acc_vertical = bad_vert_accel;
}
}
void Ekf::controlHeightFusion()
{
checkVerticalAccelerationHealth();