diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index e469296e8a..cabf245190 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -950,6 +950,8 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, extNavMeasTime_ms = timeStamp_ms; } + ext_nav_elements extNavDataNew {}; + if (resetTime_ms != extNavLastPosResetTime_ms) { extNavDataNew.posReset = true; extNavLastPosResetTime_ms = resetTime_ms; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index a8d8469944..2934d69f90 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -395,8 +395,7 @@ void NavEKF3_core::InitialiseVariables() memset(&yawAngDataDelayed, 0, sizeof(yawAngDataDelayed)); // external nav data fusion - memset(&extNavDataNew, 0, sizeof(extNavDataNew)); - memset(&extNavDataDelayed, 0, sizeof(extNavDataDelayed)); + extNavDataDelayed = {}; extNavMeasTime_ms = 0; extNavLastPosResetTime_ms = 0; lastExtNavPassTime_ms = 0; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index a873bdc5e3..4d6fa874e9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1329,7 +1329,6 @@ private: // external navigation fusion obs_ring_buffer_t storedExtNav; // external navigation data buffer - ext_nav_elements extNavDataNew; // External nav data at the current time horizon ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec) uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec)