diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 091c176a3a..911539f0dc 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -118,8 +118,11 @@ void AP_AHRS_NavEKF::update_EKF1(void) start_time_ms = AP_HAL::millis(); } // slight extra delay on EKF1 to prioritise EKF2 for memory - if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100U) { + if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100U || force_ekf) { ekf1_started = EKF1.InitialiseFilterDynamic(); + if (force_ekf) { + return; + } } } if (ekf1_started) { @@ -189,8 +192,11 @@ 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) { + if (AP_HAL::millis() - start_time_ms > startup_delay_ms || force_ekf) { ekf2_started = EKF2.InitialiseFilter(); + if (force_ekf) { + return; + } } } if (ekf2_started) { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 21115bc070..486e1c540c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -217,6 +217,9 @@ public: bool getGpsGlitchStatus(); + // used by Replay to force start at right timestamp + void force_ekf_start(void) { force_ekf = true; } + private: enum EKF_TYPE {EKF_TYPE_NONE=0, #if AP_AHRS_WITH_EKF1 @@ -235,8 +238,9 @@ private: NavEKF &EKF1; NavEKF2 &EKF2; - bool ekf1_started = false; - bool ekf2_started = false; + bool ekf1_started:1; + bool ekf2_started:1; + bool force_ekf:1; Matrix3f _dcm_matrix; Vector3f _dcm_attitude; Vector3f _gyro_bias;