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

AP_NavEKF : Change mag fusion to use corrected compass values 

This enables compass units to be switched in-flight
This commit is contained in:
priseborough 2014-10-17 16:46:46 +11:00 committed by Andrew Tridgell
parent c3be486c29
commit 9132fcfe43
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3922,8 +3922,7 @@ void NavEKF::readMagData()
// store time of last measurement update
lastMagUpdate = _ahrs->get_compass()->last_update;
// Read compass data
// We scale compass data to improve numerical conditioning
// read compass data and scale to improve numerical conditioning
magData = _ahrs->get_compass()->get_field() * 0.001f;
// get states stored at time closest to measurement time after allowance for measurement delay
@ -4054,7 +4053,7 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
// calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn);
initMagNED = Tbn* magData;
initMagNED = Tbn * magData;
// write to earth magnetic field state vector
state.earth_magfield = initMagNED;