aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Improve checking of time delay validity when setting buffers

Browse Source 
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.
This commit is contained in:
priseborough 2017-05-22 11:14:57 +10:00 committed by Francisco Ferreira
parent 7d63286088
commit 7659e9c31f
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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