AP_NavEKF3: delay startup until IMU buffer is filled

this prevents a vulnerability where the initial IMU data is processed
26 times (where 26 is the IMU buffer length)

libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h

@@ -147,8 +147,16 @@ public:
         buffer[_youngest].element = element;
         // set oldest data index
         _oldest = (_youngest+1)%_size;
+        if (_oldest == 0) {
+            _filled = true;
+        }
     }
 
+    // return true if the buffer has been filled at least once
+    inline bool is_filled(void) const {
+        return _filled;
+    }
+    
     // retrieve the oldest data from the ring buffer tail
     inline element_type pop_oldest_element() {
         element_type ret = buffer[_oldest].element;
@@ -185,4 +193,5 @@ public:
     }
 private:
     uint8_t _size,_oldest,_youngest;
+    bool _filled;
 };

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -408,6 +408,13 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     readGpsData();
     readBaroData();
 
+    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
+        return storedIMU.is_filled();
+    }
+
     // accumulate enough sensor data to fill the buffers
     if (firstInitTime_ms == 0) {
         firstInitTime_ms = imuSampleTime_ms;
@@ -469,7 +476,8 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     statesInitialised = true;
     gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initialised",(unsigned)imu_index);
 
-    return true;
+    // we initially return false to wait for the IMU buffer to fill
+    return false;
 }
 
 // initialise the covariance matrix