diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h b/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h index d9ef1c618d..938f93a351 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h @@ -147,6 +147,13 @@ public: buffer[_youngest].element = element; // set oldest data index _oldest = (_youngest+1)%_size; + if (_oldest == 0) { + _filled = true; + } + } + + inline bool is_filled(void) const { + return _filled; } // retrieve the oldest data from the ring buffer tail @@ -185,4 +192,5 @@ public: } private: uint8_t _size,_oldest,_youngest; + bool _filled; }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 08e1fdf891..89e7be7757 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -341,6 +341,17 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) return false; } + if (statesInitialised) { + // we are initialised, but we don't return true until the IMU + // buffer has been filled. This prevents a timing + // vulnerability with a pause in IMU data during filter startup + readIMUData(); + readMagData(); + readGpsData(); + readBaroData(); + return storedIMU.is_filled(); + } + // set re-used variables to zero InitialiseVariables(); @@ -410,7 +421,8 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) // set to true now that states have be initialised statesInitialised = true; - return true; + // we initially return false to wait for the IMU buffer to fill + return false; } // initialise the covariance matrix