Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bug in declination fusion

This commit is contained in:
priseborough 2017-06-03 20:03:51 +10:00 committed by Randy Mackay
parent b4c1ecf9ce
commit 46b4c55fdc
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -1006,7 +1006,7 @@ void NavEKF2_core::FuseDeclination(float declErr)
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0; float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// Calculate the innovation // Calculate the innovation
float innovation = atanf(t4) - magDecAng; float innovation = atan2f(magE , magN) - magDecAng;
// limit the innovation to protect against data errors // limit the innovation to protect against data errors
if (innovation > 0.5f) { if (innovation > 0.5f) {