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

EKF: Add support for use of GPS heading data.

Browse Source 
Heading data is assumed to be from a  dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned.
This commit is contained in:
Paul Riseborough
2018-09-19 06:32:43 +10:00
committed by Paul Riseborough
parent af60e6bbc8
commit 91f886cb5e
8 changed files with 499 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+9
View File
@@ -545,6 +545,15 @@ bool Ekf::realignYawGPS()
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
// do not use the magnetomer and deactivate magnetic field states
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
return false;
}
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quatf quat_before_reset = _state.quat_nominal;
Quatf quat_after_reset = _state.quat_nominal;