From 560f17a570ab1bccb19e17847193583ec58121ca Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 1 Oct 2018 09:53:31 +1000 Subject: [PATCH] AP_NavEKF3: use union to alias array and struct access to states This avoids creating two pointers of different types to the same memory. Having two pointers to the same memory can lead to the compiler optimising code such that a write to one pointer is rearranged to be either before or after a read from the other pointer depending on which is deemed faster - not a good outcome. --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 -- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 8 ++++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 32b78ff463..1be2ecf208 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -12,8 +12,6 @@ extern const AP_HAL::HAL& hal; // constructor NavEKF3_core::NavEKF3_core(void) : - stateStruct(*reinterpret_cast(&statesArray)), - _perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_UpdateFilter")), _perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_CovariancePrediction")), _perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseVelPosNED")), diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 3e79c6b7ed..79c43d5759 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -415,7 +415,6 @@ private: // the states are available in two forms, either as a Vector24, or // broken down as individual elements. Both are equivalent (same // memory) - Vector24 statesArray; struct state_elements { Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame Vector3f velocity; // velocity of IMU in local NED earth frame (m/sec) @@ -425,7 +424,12 @@ private: Vector3f earth_magfield; // earth frame magnetic field vector (Gauss) Vector3f body_magfield; // body frame magnetic field vector (Gauss) Vector2f wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) - } &stateStruct; + }; + + union { + Vector24 statesArray; + struct state_elements stateStruct; + }; struct output_elements { Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame