AP_NavEKF: Set prevDelAng after using it

ArduPilot · Oct 30, 2014 · bb6d8fd · bb6d8fd
1 parent aca78d3
commit bb6d8fd
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -856,13 +856,13 @@ void NavEKF::UpdateStrapdownEquationsNED()
     // use weighted average of both IMU units for delta velocities
     correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting);
 
-    // save current measurements
-    prevDelAng = correctedDelAng;
-
     // apply corrections for earths rotation rate and coning errors
     // % * - and + operators have been overloaded
     correctedDelAng   = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f;
 
+    // save current measurements
+    prevDelAng = correctedDelAng;
+
     // convert the rotation vector to its equivalent quaternion
     rotationMag = correctedDelAng.length();
     if (rotationMag < 1e-12f)