fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 15:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)

Browse Source
This commit is contained in:
Daniel Agar
2023-03-21 11:25:34 -04:00
parent 0784901a66
commit 32a5bd32ad
17 changed files with 343 additions and 173 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+12 -6
View File
@@ -69,16 +69,17 @@ void Ekf::initialiseCovariance()
// position
P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f));
P(8,8) = P(7,7);
P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
if (_control_status.flags.gps_hgt) {
P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
}
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_control_status.flags.rng_hgt) {
P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
} else if (_control_status.flags.gps_hgt) {
P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
} else {
P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
}
#endif // CONFIG_EKF2_RANGE_FINDER
// gyro bias
_prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
@@ -453,7 +454,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
if (bad_vz_gps || bad_vz_ev) {
bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
#if defined(CONFIG_EKF2_RANGE_FINDER)
bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f);
#else
bool bad_z_rng = false;
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f);