EKF: Fix race condition in accel measurement assignment

1 files changed, 9 insertions, 6 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index b062097fb..6cff4b948 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -181,9 +181,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     correctedDelAng.x = dAngIMU.x - states[10];
     correctedDelAng.y = dAngIMU.y - states[11];
     correctedDelAng.z = dAngIMU.z - states[12];
-    dVelIMU.x = dVelIMU.x;
-    dVelIMU.y = dVelIMU.y;
-    dVelIMU.z = dVelIMU.z - states[13];
+
+    Vector3f dVelIMURel;
+
+    dVelIMURel.x = dVelIMU.x;
+    dVelIMURel.y = dVelIMU.y;
+    dVelIMURel.z = dVelIMU.z - states[13];
 
     delAngTotal.x += correctedDelAng.x;
     delAngTotal.y += correctedDelAng.y;
@@ -263,9 +266,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
 // transform body delta velocities to delta velocities in the nav frame
 // * and + operators have been overloaded
     //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
-    delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
-    delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
-    delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
+    delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
+    delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
+    delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;
 
 // calculate the magnitude of the nav acceleration (required for GPS
 // variance estimation)