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