AP_NavEKF2: don't run IMU updates until buffer fills

this prevents us using bad initial data multiple times. It fixes a bug
where the IMU may pause during EK2 initialisation

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;
 };

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