fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-19 02:50:35 +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/ekf_helper.cpp
+31 -10
View File
@@ -206,7 +206,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
#endif // CONFIG_EKF2_RANGE_FINDER
@@ -293,6 +295,7 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
}
#if defined(CONFIG_EKF2_GNSS)
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
{
hvel[0] = _aid_src_gnss_vel.innovation[0];
@@ -323,6 +326,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
vpos = _aid_src_gnss_hgt.test_ratio;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
@@ -440,11 +444,15 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
// determine current z
float current_alt = -_state.pos(2) + gps_alt_ref_prev;
#if defined(CONFIG_EKF2_GNSS)
const float gps_hgt_bias = _gps_hgt_b_est.getBias();
#endif // CONFIG_EKF2_GNSS
resetVerticalPositionTo(_gps_alt_ref - current_alt);
#if defined(CONFIG_EKF2_GNSS)
// preserve GPS height bias
_gps_hgt_b_est.setBias(gps_hgt_bias);
#endif // CONFIG_EKF2_GNSS
}
return true;
@@ -465,9 +473,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_control_status.flags.inertial_dead_reckoning) {
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_pos) {
@@ -490,9 +500,11 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_horizontal_deadreckon_time_exceeded) {
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_pos) {
@@ -523,9 +535,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_pos) {
@@ -670,6 +684,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
vel = NAN;
pos = NAN;
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max());
vel = math::max(gps_vel, FLT_MIN);
@@ -677,6 +692,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max());
pos = math::max(gps_pos, FLT_MIN);
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_vel) {
@@ -708,10 +724,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps_hgt) {
hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio);
n_hgt_sources++;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_control_status.flags.rng_hgt) {
@@ -795,10 +813,15 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_GNSS)
const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
#else
(void)mag_innov_good;
#endif // CONFIG_EKF2_GNSS
soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
*status = soln_status.value;
}
@@ -995,6 +1018,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
_time_last_heading_fuse = _time_delayed_us;
}
#if defined(CONFIG_EKF2_GNSS)
bool Ekf::resetYawToEKFGSF()
{
if (!isYawEmergencyEstimateAvailable()) {
@@ -1019,9 +1043,11 @@ bool Ekf::resetYawToEKFGSF()
return true;
}
#endif // CONFIG_EKF2_GNSS
bool Ekf::isYawEmergencyEstimateAvailable() const
{
#if defined(CONFIG_EKF2_GNSS)
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
if (!_yawEstimator.isActive()) {
@@ -1029,23 +1055,18 @@ bool Ekf::isYawEmergencyEstimateAvailable() const
}
return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
#else
return false;
#endif
}
#if defined(CONFIG_EKF2_GNSS)
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
}
void Ekf::resetGpsDriftCheckFilters()
{
_gps_velNE_filt.setZero();
_gps_pos_deriv_filt.setZero();
_gps_horizontal_position_drift_rate_m_s = NAN;
_gps_vertical_position_drift_rate_m_s = NAN;
_gps_filtered_horizontal_velocity_m_s = NAN;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWind()