AP_AHRS: delay EKF start until logging ready

for replay to work reliably we need all the parameters output before
we init the EKF.

Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -26,6 +26,7 @@
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Baro/AP_Baro.h>
 #include <AP_InternalError/AP_InternalError.h>
+#include <AP_Logger/AP_Logger.h>
 #include <AP_Notify/AP_Notify.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 
@@ -194,12 +195,19 @@ void AP_AHRS_NavEKF::update_EKF2(void)
         if (start_time_ms == 0) {
             start_time_ms = AP_HAL::millis();
         }
-        if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) {
-            _ekf2_started = EKF2.InitialiseFilter();
-            if (_force_ekf) {
-                return;
+        // if we're doing Replay logging then don't allow any data
+        // into the EKF yet.  Don't allow it to block us for long.
+        if (!hal.util->was_watchdog_reset()) {
+            if (AP_HAL::millis() - start_time_ms < 5000) {
+                if (!AP::logger().allow_start_ekf()) {
+                    return;
+                }
             }
         }
+
+        if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
+            _ekf2_started = EKF2.InitialiseFilter();
+        }
     }
     if (_ekf2_started) {
         EKF2.UpdateFilter();
@@ -268,12 +276,18 @@ void AP_AHRS_NavEKF::update_EKF3(void)
         if (start_time_ms == 0) {
             start_time_ms = AP_HAL::millis();
         }
-        if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) {
-            _ekf3_started = EKF3.InitialiseFilter();
-            if (_force_ekf) {
-                return;
+        // if we're doing Replay logging then don't allow any data
+        // into the EKF yet.  Don't allow it to block us for long.
+        if (!hal.util->was_watchdog_reset()) {
+            if (AP_HAL::millis() - start_time_ms < 5000) {
+                if (!AP::logger().allow_start_ekf()) {
+                    return;
+                }
             }
         }
+        if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
+            _ekf3_started = EKF3.InitialiseFilter();
+        }
     }
     if (_ekf3_started) {
         EKF3.UpdateFilter();

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -277,9 +277,6 @@ public:
 
     bool getGpsGlitchStatus() const;
 
-    // used by Replay to force start at right timestamp
-    void force_ekf_start(void) { _force_ekf = true; }
-
     // is the EKF backend doing its own sensor logging?
     bool have_ekf_logging(void) const override;
 
@@ -348,8 +345,7 @@ private:
     bool _ekf3_started;
     void update_EKF3(void);
 #endif
-    bool _force_ekf;
-    
+
     // rotation from vehicle body to NED frame
     Matrix3f _dcm_matrix;
     Vector3f _dcm_attitude;