fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 19:53: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/airspeed_fusion.cpp
+8 -1
View File
@@ -62,6 +62,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_control_status.flags.wind = false;
}
#if defined(CONFIG_EKF2_GNSS)
// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
if (_control_status.flags.fixed_wing) {
if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
@@ -73,6 +74,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_yawEstimator.setTrueAirspeed(0.f);
}
}
#endif // CONFIG_EKF2_GNSS
if (_params.arsp_thr <= 0.f) {
stopAirspeedFusion();
@@ -99,7 +101,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
fuseAirspeed(airspeed_sample, _aid_src_airspeed);
}
#if defined(CONFIG_EKF2_GNSS)
_yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed);
#endif // CONFIG_EKF2_GNSS
const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
@@ -219,8 +223,11 @@ void Ekf::stopAirspeedFusion()
if (_control_status.flags.fuse_aspd) {
ECL_INFO("stopping airspeed fusion");
resetEstimatorAidStatus(_aid_src_airspeed);
_yawEstimator.setTrueAirspeed(NAN);
_control_status.flags.fuse_aspd = false;
#if defined(CONFIG_EKF2_GNSS)
_yawEstimator.setTrueAirspeed(NAN);
#endif // CONFIG_EKF2_GNSS
}
}