From ceb42225fd61dd636ef0902ef24a510497a1b6e8 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Mon, 19 Dec 2016 11:39:37 +1100
Subject: [PATCH] AP_NavEKF3: Ensure enough time to fill buffers when starting
 filter

---
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 33 +++++++++++++-----------
 libraries/AP_NavEKF3/AP_NavEKF3_core.h   |  1 +
 2 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 0ee24eec08..b0e3a53297 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -35,6 +35,7 @@ NavEKF3_core::NavEKF3_core(void) :
     _perf_test[7] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test7");
     _perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test8");
     _perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test9");
+    firstInitTime_ms = 0;
 }
 
 // setup this core backend
@@ -355,24 +356,29 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
         return false;
     }
 
+    // read all the sensors required to start the EKF the states
+    readIMUData();
+    readMagData();
+    readGpsData();
+    readBaroData();
+
+    // accumulate enough sensor data to fill the buffers
+    if (firstInitTime_ms == 0) {
+        firstInitTime_ms = imuSampleTime_ms;
+        return false;
+    } else if (imuSampleTime_ms - firstInitTime_ms < 1000) {
+        return false;
+    }
+
     // set re-used variables to zero
     InitialiseVariables();
 
-    // Initialise IMU data
-    dtIMUavg = _ahrs->get_ins().get_loop_delta_t();
-    readIMUData();
-    storedIMU.reset_history(imuDataNew);
-    imuDataDelayed = imuDataNew;
-
     // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
     Vector3f initAccVec;
 
     // TODO we should average accel readings over several cycles
     initAccVec = _ahrs->get_ins().get_accel(imu_index);
 
-    // read the magnetometer data
-    readMagData();
-
     // normalise the acceleration vector
     float pitch=0, roll=0;
     if (initAccVec.length() > 0.001f) {
@@ -399,13 +405,9 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     stateStruct.earth_magfield.zero();
     stateStruct.body_magfield.zero();
 
-    // read the GPS and set the position and velocity states
-    readGpsData();
+    // set the position, velocity and height
     ResetVelocity();
     ResetPosition();
-
-    // read the barometer and set the height state
-    readBaroData();
     ResetHeight();
 
     // define Earth rotation vector in the NED navigation frame
@@ -414,11 +416,12 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     // initialise the covariance matrix
     CovarianceInit();
 
-    // reset output states
+    // reset the output predictor states
     StoreOutputReset();
 
     // set to true now that states have be initialised
     statesInitialised = true;
+    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initialised",(unsigned)imu_index);
 
     return true;
 }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index c86583a659..c5296cb055 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -896,6 +896,7 @@ private:
     Vector3f outputTrackError;      // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
     Vector3f velOffsetNED;          // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
     Vector3f posOffsetNED;          // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
+    uint32_t firstInitTime_ms;      // First time the initialise function was called (msec)
 
     // Specify preferred source of data to be used for a state reset
     enum resetDataSource {