Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Fixes zero compass offsets on initialisation

Browse Source
This commit is contained in:
Paul Riseborough 2014-02-28 18:54:07 +11:00 committed by Andrew Tridgell
parent cc4c443b32
commit 854f013146
1 changed files with 15 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -348,12 +348,11 @@ void NavEKF::InitialiseFilterDynamic(void)
// calculate rotation matrix from body to NED frame
Tbn.from_euler(_ahrs->roll, _ahrs->pitch, 0.0f);
// body magnetic field vector with offsets removed
Vector3f initMagXYZ;
initMagXYZ = _ahrs->get_compass()->get_field() * 0.001f; // convert from Gauss to mGauss
// read the magnetometer data
readMagData();
// rotate the magnetic field into NED axes
initMagNED = Tbn*initMagXYZ;
initMagNED = Tbn*magData;
// calculate heading of mag field rel to body heading
float magHeading = atan2f(initMagNED.y, initMagNED.x);
@ -371,7 +370,7 @@ void NavEKF::InitialiseFilterDynamic(void)
// Calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn);
initMagNED = Tbn * initMagXYZ;
initMagNED = Tbn * magData;
// write to state vector
state.quat = initQuat;
@ -404,12 +403,11 @@ void NavEKF::InitialiseFilterBootstrap(void)
// acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f initAccVec;
// body magnetic field vector with offsets removed
Vector3f initMagXYZ;
// we should average readings over several calls to this function
// TODO we should average accel readings over several cycles
initAccVec = _ahrs->get_ins().get_accel();
initMagXYZ = _ahrs->get_compass()->get_field() * 0.001f; // convert from Gauss to mGauss
// read the magnetometer data
readMagData();
// Normalise the acceleration vector
initAccVec.normalize();
@ -423,16 +421,16 @@ void NavEKF::InitialiseFilterBootstrap(void)
// calculate initial yaw angle
float yaw;
Matrix3f Tbn;
Vector3f initMagVecNED;
Vector3f initMagNED;
if (useCompass) {
// calculate rotation matrix from body to NED frame
Tbn.from_euler(roll, pitch, 0.0f);
// rotate the magnetic field into NED axesn
initMagVecNED = Tbn*initMagXYZ;
initMagNED = Tbn*magData;
// calculate heading of mag field rel to body heading
float magHeading = atan2f(initMagVecNED.y, initMagVecNED.x);
float magHeading = atan2f(initMagNED.y, initMagNED.x);
// get the magnetic declination
float magDecAng = _ahrs->get_compass()->get_declination();
@ -450,10 +448,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
// Calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn);
initMagVecNED = Tbn * initMagXYZ;
//Get the initial compass bias estimates
Vector3f initMagBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
initMagNED = Tbn * magData;
// read the GPS
readGpsData();
@ -469,8 +464,8 @@ void NavEKF::InitialiseFilterBootstrap(void)
state.gyro_bias.zero();
state.accel_zbias = 0;
state.wind_vel.zero();
state.earth_magfield = initMagVecNED;
state.body_magfield = initMagBias;
state.earth_magfield = initMagNED;
state.body_magfield = magBias;
statesInitialised = true;
@ -2255,7 +2250,7 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
void NavEKF::getAccelBias(Vector3f &accelBias) const
{
accelBias.x = staticMode? 1.0f : 0.0f;
accelBias.y = static_mode_demanded()? 1.0f : 0.0f;
accelBias.y = onGround? 1.0f : 0.0f;
accelBias.z = states[13] / dtIMU;
}