From bb6d8fd44acb3ef9ae9a1a753c4a2c0ca94fa00e Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sun, 12 Oct 2014 04:11:05 -0700 Subject: [PATCH] AP_NavEKF: Set prevDelAng after using it --- libraries/AP_NavEKF/AP_NavEKF.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 376c0ec596..e18b9fabd9 100644 --- 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)