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

ekf2: fully disable yaw estimator EKFGSF_yaw with CONFIG_EKF2_GNSS (#22233)

Browse Source
This commit is contained in:
Daniel Agar
2023-10-18 13:30:17 -04:00
committed by GitHub
parent fefdad83bf
commit bdaf0acfca
7 changed files with 94 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
-50
View File
@@ -1028,56 +1028,6 @@ 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()) {
return false;
}
// don't allow reset if there's just been a yaw reset
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
if (yaw_alignment_changed || quat_reset) {
return false;
}
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
_control_status.flags.yaw_align = true;
_information_events.flags.yaw_aligned_to_imu_gps = true;
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()) {
return false;
}
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);
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWind()
{