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

AP_NavEKF2: remove un-wanted functionality from mag and yaw reset 

The function used to reset magnetic field states and yaw angle should not be used when there is no magnetometer. If it is incorrectly called without a magnetometer it should not change the attitude or field states.
This commit is contained in:
Paul Riseborough 2016-05-24 11:06:41 +10:00 committed by Andrew Tridgell
parent 11c6ea7ef6
commit 830751c0ae
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_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -1291,7 +1291,6 @@ void NavEKF2_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
// initialise the earth magnetic field states using declination, suppled roll/pitch
// and magnetometer measurements and return initial attitude quaternion
// if no magnetometer data, do not update magnetic field states and assume zero yaw angle
Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
{
// declare local variables required to calculate initial orientation and magnetic field
@ -1362,8 +1361,8 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
// clear bad magnetic yaw status
badMagYaw = false;
} else {
initQuat.from_euler(roll, pitch, 0.0f);
yawAlignComplete = false;
// no magnetoemter data so there is nothing we can do
initQuat = stateStruct.quat;
}
// return attitude quaternion