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

AP_NavEKF: Set prevDelAng after using it

Browse Source
This commit is contained in:
Jonathan Challinger 2014-10-12 04:11:05 -07:00 committed by Andrew Tridgell
parent aca78d321f
commit bb6d8fd44a
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -856,13 +856,13 @@ void NavEKF::UpdateStrapdownEquationsNED()
// use weighted average of both IMU units for delta velocities // use weighted average of both IMU units for delta velocities
correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting); correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting);
// save current measurements
prevDelAng = correctedDelAng;
// apply corrections for earths rotation rate and coning errors // apply corrections for earths rotation rate and coning errors
// % * - and + operators have been overloaded // % * - and + operators have been overloaded
correctedDelAng = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f; correctedDelAng = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f;
// save current measurements
prevDelAng = correctedDelAng;
// convert the rotation vector to its equivalent quaternion // convert the rotation vector to its equivalent quaternion
rotationMag = correctedDelAng.length(); rotationMag = correctedDelAng.length();
if (rotationMag < 1e-12f) if (rotationMag < 1e-12f)