From 7659e9c31f51218147bcd5b3cca45d7401bd3dba Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 22 May 2017 11:14:57 +1000 Subject: [PATCH] AP_NavEKF3: Improve checking of time delay validity when setting buffers This change means that provided the GPS_DELAY_MS parameters are set for each GPS receiver attached, the EKF will not have to wait for the configuration of each receiver to be determined before it can start. This significantly reduces start-up times when the delay parameters are set. --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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