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

GPS Yaw: fix heading initialisation and add unit tests

Browse Source 
When the antennas are not parallel to the x body axis, the GPS message
contains the angular offset but the data is already corrected in the
driver. EKF2 should then not add this offset during the initialisation.
This commit is contained in:
bresch
2020-07-02 13:35:13 +02:00
committed by Mathieu Bresciani
parent 97225fcb74
commit 7f4fedde6a
4 changed files with 71 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_yaw_fusion.cpp
+2 -2
View File
@@ -273,8 +273,8 @@ bool Ekf::resetGpsAntYaw()
return false;
}
// get measurement and correct for antenna array yaw offset
const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
// GPS yaw measurement is alreday compensated for antenna offset in the driver
const float measured_yaw = _gps_sample_delayed.yaw;
const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
resetQuatStateYaw(measured_yaw, yaw_variance, true);