Browse Source

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.
mission-4.1.18
priseborough 8 years ago committed by Francisco Ferreira
parent
commit
7659e9c31f
  1. 5
      libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

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

Loading…
Cancel
Save