fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:13:34 +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/test/test_EKF_gps_yaw.cpp
+3 -3
View File
@@ -204,7 +204,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
//const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
// BUT WHEN: the GPS yaw is suddenly invalid
gps_heading = NAN;
@@ -215,7 +215,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
// the estimator should fall back to mag fusion
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
//EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
}
TEST_F(EkfGpsHeadingTest, fallBackToYawEmergencyEstimator)
@@ -328,7 +328,7 @@ TEST_F(EkfGpsHeadingTest, stopOnGround)
// THEN: the fusion should stop and the GPS pos/vel aiding
// should stop as well because the yaw is not aligned anymore
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
//EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
// AND IF: the mag fusion type is set to NONE
_ekf_wrapper.setMagFuseTypeNone();