aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Enforce alignment of realigned earth mag field with declination

Browse Source 
This prevents bad inertial or GPS data combined with the post takeoff heading alignment check used by plane from resulting in earth field states that have an incorrect declination
This commit is contained in:
Paul Riseborough 2015-05-19 15:52:13 +10:00 committed by Andrew Tridgell
parent b1d8805114
commit c1c5e3598a
2 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4397,10 +4397,10 @@ 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;
state.earth_magfield = Tbn * magData;
// write to earth magnetic field state vector
state.earth_magfield = initMagNED;
// align the NE earth magnetic field states with the published declination
alignMagStateDeclination();
// clear bad magnetometer status
badMag = false;
@ -5183,4 +5183,18 @@ void NavEKF::getQuaternion(Quaternion& ret) const
ret = state.quat;
}
// align the NE earth magnetic field states with the published declination
void NavEKF::alignMagStateDeclination()
{
// get the magnetic declination
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// rotate the NE values so that the declination matches the published value
Vector3f initMagNED = state.earth_magfield;
float magLengthNE = pythagorous2(initMagNED.x,initMagNED.y);
state.earth_magfield.x = magLengthNE * cosf(magDecAng);
state.earth_magfield.y = magLengthNE * sinf(magDecAng);
}
#endif // HAL_CPU_CLASS

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -453,6 +453,9 @@ private:
// check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
void detectOptFlowTakeoff(void);
// align the NE earth magnetic field states with the published declination
void alignMagStateDeclination();
// EKF Mavlink Tuneable Parameters
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s