Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

ekf2: move gps yaw reset in starting function

Browse Source
This commit is contained in:
bresch
2021-10-25 14:01:08 +02:00
committed by Mathieu Bresciani
parent 1461eb0e32
commit 9afc390552
2 changed files with 12 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+3 -3
View File
@@ -820,10 +820,10 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
} else {
if (starting_conditions_passing) {
// Try to activate GPS yaw fusion
if (resetYawToGps()) {
_control_status.flags.yaw_align = true;
startGpsYawFusion();
if (_control_status.flags.gps_yaw) {
_nb_gps_yaw_reset_available = 1;
startGpsYawFusion();
}
}
}
src/modules/ekf2/EKF/ekf_helper.cpp
+9 -5
View File
@@ -1546,11 +1546,15 @@ void Ekf::stopGpsVelFusion()
void Ekf::startGpsYawFusion()
{
_control_status.flags.mag_dec = false;
stopEvYawFusion();
stopMagHdgFusion();
stopMag3DFusion();
_control_status.flags.gps_yaw = true;
if (resetYawToGps()) {
_control_status.flags.yaw_align = true;
_control_status.flags.mag_dec = false;
stopEvYawFusion();
stopMagHdgFusion();
stopMag3DFusion();
_control_status.flags.gps_yaw = true;
}
}
void Ekf::stopGpsYawFusion()