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