diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 830dccf7fb..41e61c7fb5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -71,7 +71,8 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c // GPS sensing can have large delays and should not be included if disabled if (_frontend->_fusionModeGPS != 3) { // Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay - if (!_ahrs->get_gps().all_configured()) { + float gps_delay_sec = 0; + if (!_ahrs->get_gps().get_lag(gps_delay_sec)) { if (AP_HAL::millis() - lastInitFailReport_ms > 10000) { lastInitFailReport_ms = AP_HAL::millis(); // provide an escalating series of messages @@ -86,7 +87,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c return false; } // limit the time delay value from the GPS library to a max of 250 msec which is the max value the EKF has been tested for. - maxTimeDelay_ms = MAX(maxTimeDelay_ms , MIN((uint16_t)(_ahrs->get_gps().get_lag() * 1000.0f),250)); + maxTimeDelay_ms = MAX(maxTimeDelay_ms , MIN((uint16_t)(gps_delay_sec * 1000.0f),250)); } // airspeed sensing can have large delays and should not be included if disabled