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

ekf2: GPS yaw only invalidate yaw_align if stopping due to fusion failure

Browse Source
This commit is contained in:
Daniel Agar
2024-04-29 21:04:21 -04:00
committed by GitHub
parent 547209e1dc
commit b6da0b141d
2 changed files with 11 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_control.cpp
+8 -9
View File
@@ -366,6 +366,13 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
if (is_fusion_failing) {
stopGpsYawFusion();
// Before takeoff, we do not want to continue to rely on the current heading
// if we had to stop the fusion
if (!_control_status.flags.in_air) {
ECL_INFO("clearing yaw alignment");
_control_status.flags.yaw_align = false;
}
}
} else {
@@ -416,15 +423,7 @@ void Ekf::stopGpsYawFusion()
_control_status.flags.gps_yaw = false;
resetEstimatorAidStatus(_aid_src_gnss_yaw);
// Before takeoff, we do not want to continue to rely on the current heading
// if we had to stop the fusion
if (!_control_status.flags.in_air) {
ECL_INFO("stopping GPS yaw fusion, clearing yaw alignment");
_control_status.flags.yaw_align = false;
} else {
ECL_INFO("stopping GPS yaw fusion");
}
ECL_INFO("stopping GPS yaw fusion");
}
}
#endif // CONFIG_EKF2_GNSS_YAW