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

GNSS yaw: unset yaw_align if fusion stops before takeoff

Browse Source 
We do not want to rely on a potentially badly initialized heading as it
could lead to a flyaway directly after takeoff

Also remove the resetMagHeading() call from the GPS fusion control logic
as this is properly handled in mag_control.
This commit is contained in:
bresch
2021-05-12 08:33:07 +10:00
committed by Paul Riseborough
parent 7ee69706e8
commit a685987082
3 changed files with 70 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_gps_yaw.cpp
+19
View File
@@ -239,3 +239,22 @@ TEST_F(EkfGpsHeadingTest, yaw_jump)
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
}
TEST_F(EkfGpsHeadingTest, stop_on_ground)
{
// GIVEN: the GPS yaw fusion activated and there is no mag data
_sensor_simulator._mag.stop();
float gps_heading = _ekf_wrapper.getYawAngle();
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(5);
// WHEN: the measurement stops
gps_heading = NAN;
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(7.5);
// 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());
}