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

ekf2: new kconfig to enable/disable GNSS (enabled by default)

Browse Source
This commit is contained in:
Daniel Agar
2023-10-11 14:02:34 -04:00
committed by GitHub
parent 2d78383296
commit d2b3e7fe16
24 changed files with 476 additions and 331 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+17 -2
View File
@@ -57,16 +57,24 @@ void Ekf::initialiseCovariance()
resetQuatCov();
// velocity
#if defined(CONFIG_EKF2_GNSS)
const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f));
#else
const float vel_var = sq(0.5f);
#endif
P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var));
// position
const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
#if defined(CONFIG_EKF2_GNSS)
const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
if (_control_status.flags.gps_hgt) {
z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
}
#else
const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f));
#endif
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_control_status.flags.rng_hgt) {
@@ -404,8 +412,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
bool bad_acc_bias = false;
if (fabsf(down_dvel_bias) > dVel_bias_lim) {
#if defined(CONFIG_EKF2_GNSS)
bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
#else
bool bad_vz_gps = false;
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f);
#else
@@ -418,7 +429,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
#else
bool bad_z_baro = false;
#endif
#if defined(CONFIG_EKF2_GNSS)
bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
#else
bool bad_z_gps = false;
#endif
#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);