@ -4397,10 +4397,10 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
@@ -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
@@ -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