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,13 +195,20 @@ 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) {
+        // if we're doing Replay logging then don't allow any data
-            _ekf2_started = EKF2.InitialiseFilter();
+        // into the EKF yet.  Don't allow it to block us for long.
-            if (_force_ekf) {
+        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();
         if (active_EKF_type() == EKFType::TWO) {
@@ -268,13 +276,19 @@ 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) {
+        // if we're doing Replay logging then don't allow any data
-            _ekf3_started = EKF3.InitialiseFilter();
+        // into the EKF yet.  Don't allow it to block us for long.
-            if (_force_ekf) {
+        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();
         if (active_EKF_type() == EKFType::THREE) {

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,7 +345,6 @@ private:
     bool _ekf3_started;
     void update_EKF3(void);
 #endif
-    bool _force_ekf;
 
     // rotation from vehicle body to NED frame
     Matrix3f _dcm_matrix;