diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 67ea03c597..e0f7cb4837 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -529,6 +529,7 @@ private: const uint16_t fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec const uint8_t sensorIntervalMin_ms = 50; // The minimum allowed time between measurements from any non-IMU sensor (msec) const uint8_t flowIntervalMin_ms = 20; // The minimum allowed time between measurements from optical flow sensors (msec) + const uint8_t extNavIntervalMin_ms = 20; // The minimum allowed time between measurements from external navigation sensors (msec) struct { bool enabled:1; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 34a8702c12..33b1f7974c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -947,7 +947,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, { // limit update rate to maximum allowed by sensor buffers and fusion process // don't try to write to buffer until the filter has been initialised - if (((timeStamp_ms - extNavMeasTime_ms) < 70) || !statesInitialised) { + if (((timeStamp_ms - extNavMeasTime_ms) < frontend->extNavIntervalMin_ms) || !statesInitialised) { return; } else { extNavMeasTime_ms = timeStamp_ms; @@ -990,7 +990,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) { - if ((timeStamp_ms - extNavVelMeasTime_ms) < 70) { + if ((timeStamp_ms - extNavVelMeasTime_ms) < frontend->extNavIntervalMin_ms) { return; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index cd042e3b0f..20531c4603 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -108,6 +108,12 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) // calculate buffer size for optical flow data const uint8_t flow_buffer_length = MIN((ekf_delay_ms / frontend->flowIntervalMin_ms) + 1, imu_buffer_length); + // calculate buffer size for external nav data + const uint8_t extnav_buffer_length = MIN((ekf_delay_ms / frontend->extNavIntervalMin_ms) + 1, imu_buffer_length); + + // buffer size for external yaw + const uint8_t yaw_angle_buffer_length = MAX(obs_buffer_length, extnav_buffer_length); + if(!storedGPS.init(obs_buffer_length)) { return false; } @@ -129,7 +135,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) if(!storedWheelOdm.init(imu_buffer_length)) { // initialise to same length of IMU to allow for multiple wheel sensors return false; } - if(!storedYawAng.init(obs_buffer_length)) { + if(!storedYawAng.init(yaw_angle_buffer_length)) { return false; } // Note: the use of dual range finders potentially doubles the amount of data to be stored @@ -140,10 +146,10 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) if(!storedRangeBeacon.init(imu_buffer_length)) { return false; } - if (!storedExtNav.init(obs_buffer_length)) { + if (!storedExtNav.init(extnav_buffer_length)) { return false; } - if (!storedExtNavVel.init(obs_buffer_length)) { + if (!storedExtNavVel.init(extnav_buffer_length)) { return false; } if(!storedIMU.init(imu_buffer_length)) {