From ed5039823f975dfad19b66570cc7b2c57b54c171 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 18 Dec 2016 09:02:38 +1100 Subject: [PATCH] AP_NavEKF3: Adapt sensor buffer lengths Adapt the lengths of the IMU and observations buffers on startup to the specified time delays and update rates. This does require the EKF to be re-started if time delays are changed. --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 9 +-- libraries/AP_NavEKF3/AP_NavEKF3.h | 3 +- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 27 ++++---- .../AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 8 ++- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 65 +++++++++++++------ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 5 +- 6 files changed, 74 insertions(+), 43 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 1e5d4befff..ef202753fd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -184,7 +184,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: GPS_DELAY // @DisplayName: GPS measurement delay (msec) - // @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements. + // @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements. The autpilot should be restarted after adjusting this parameter. // @Range: 0 250 // @Increment: 10 // @User: Advanced @@ -219,7 +219,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: HGT_DELAY // @DisplayName: Height measurement delay (msec) - // @Description: This is the number of msec that the Height measurements lag behind the inertial measurements. + // @Description: This is the number of msec that the Height measurements lag behind the inertial measurements. The autpilot should be restarted after adjusting this parameter. // @Range: 0 250 // @Increment: 10 // @User: Advanced @@ -320,7 +320,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: FLOW_DELAY // @DisplayName: Optical Flow measurement delay (msec) - // @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor. + // @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor. The autpilot should be restarted after adjusting this parameter. // @Range: 0 250 // @Increment: 10 // @User: Advanced @@ -501,7 +501,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: BCN_DELAY // @DisplayName: Range beacon measurement delay (msec) - // @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor. + // @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements. The autpilot should be restarted after adjusting this parameter. // @Range: 0 250 // @Increment: 10 // @User: Advanced @@ -559,6 +559,7 @@ NavEKF3::NavEKF3(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) : gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect gndGradientSigma(50), // RMS terrain gradient percentage assumed by the terrain height estimation fusionTimeStep_ms(10), // The minimum number of msec between covariance prediction and fusion operations + sensorIntervalMin_ms(50), // The minimum allowed time between measurements from any non-IMU sensor (msec) runCoreSelection(false) // true when the default primary core has stabilised after startup and core selection can run { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index db2d786131..d391a38143 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -386,7 +386,8 @@ private: const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active const uint8_t gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation - const uint8_t fusionTimeStep_ms; // The minimum time interval between covariance predictions and measurement fusions in msec + const uint16_t fusionTimeStep_ms; // The minimum time interval between covariance predictions and measurement fusions in msec + const uint8_t sensorIntervalMin_ms; // The minimum allowed time between measurements from any non-IMU sensor (msec) struct { bool enabled:1; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 471954173a..98a5eac8fb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -26,9 +26,8 @@ void NavEKF3_core::readRangeFinder(void) // get theoretical correct range when the vehicle is on the ground rngOnGnd = frontend->_rng.ground_clearance_cm() * 0.01f; - // read range finder at 20Hz - // TODO better way of knowing if it has new data - if ((imuSampleTime_ms - lastRngMeasTime_ms) > 50) { + // limit update rate to maximum allowed by data buffers + if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) { // reset the timer used to control the measurement rate lastRngMeasTime_ms = imuSampleTime_ms; @@ -112,6 +111,11 @@ void NavEKF3_core::readRangeFinder(void) // this needs to be called externally. void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset) { + // limit update rate to maximum allowed by sensor buffers + if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) { + return; + } + // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update // The PX4Flow sensor outputs flow rates with the following axis and sign conventions: // A positive X rate is produced by a positive sensor rotation about the X axis @@ -168,8 +172,6 @@ void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms); // Save data to buffer storedOF.push(ofDataNew); - // Check for data at the fusion time horizon - flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); } } @@ -193,9 +195,8 @@ void NavEKF3_core::readMagData() return; } - // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading - // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer - if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) { + // limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer + if (use_compass() && ((_ahrs->get_compass()->last_update_usec() - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) { frontend->logging.log_compass = true; // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available @@ -396,8 +397,8 @@ bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d void NavEKF3_core::readGpsData() { // check for new GPS data - // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer - if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > 70) { + // limit update rate to avoid overflowing the FIFO buffer + if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) { if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { // report GPS fix status gpsCheckStatus.bad_fix = false; @@ -538,8 +539,8 @@ bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { void NavEKF3_core::readBaroData() { // check to see if baro measurement has changed so we know if a new measurement has arrived - // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer - if (frontend->_baro.get_last_update() - lastBaroReceived_ms > 70) { + // limit update rate to avoid overflowing the FIFO buffer + if (frontend->_baro.get_last_update() - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) { frontend->logging.log_baro = true; baroDataNew.hgt = frontend->_baro.get_altitude(); @@ -633,7 +634,7 @@ void NavEKF3_core::readAirSpdData() const AP_Airspeed *aspeed = _ahrs->get_airspeed(); if (aspeed && aspeed->use() && - aspeed->last_update_ms() != timeTasReceived_ms) { + (aspeed->last_update_ms() - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) { tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS(); timeTasReceived_ms = aspeed->last_update_ms(); tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index c970d5580b..3d2d84f8fb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -22,6 +22,12 @@ extern const AP_HAL::HAL& hal; // select fusion of optical flow measurements void NavEKF3_core::SelectFlowFusion() { + // start performance timer + hal.util->perf_begin(_perf_FuseOptFlow); + + // Check for data at the fusion time horizon + flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); + // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz // If so, don't fuse measurements on this time step to reduce frame over-runs // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements @@ -32,8 +38,6 @@ void NavEKF3_core::SelectFlowFusion() optFlowFusionDelayed = false; } - // start performance timer - hal.util->perf_begin(_perf_FuseOptFlow); // Perform Data Checks // Check if the optical flow data is still valid flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 8285ef94f1..183e4e38d2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -8,8 +8,7 @@ #include "AP_NavEKF3_core.h" #include #include - -#include +#include extern const AP_HAL::HAL& hal; @@ -47,34 +46,62 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c _ahrs = frontend->_ahrs; /* - the imu_buffer_length needs to cope with a 260ms delay at a - maximum fusion rate of 100Hz. Non-imu data coming in at faster - than 100Hz is downsampled. For 50Hz main loop rate we need a - shorter buffer. + The imu_buffer_length needs to cope with the worst case sensor delay at the + maximum fusion rate of 100Hz. Non-imu data coming in at faster than 100Hz is + downsampled. For 50Hz main loop rate we need a shorter buffer. */ - if (_ahrs->get_ins().get_sample_rate() < 100) { - imu_buffer_length = 13; + + // Calculate the expected EKF time step + if (_ahrs->get_ins().get_sample_rate() > 0) { + dtEkfAvg = 1.0f / _ahrs->get_ins().get_sample_rate(); + dtEkfAvg = fmaxf(dtEkfAvg,EKF_TARGET_DT); } else { - // maximum 260 msec delay at 100 Hz fusion rate - imu_buffer_length = 26; - } - if(!storedGPS.init(OBS_BUFFER_LENGTH)) { return false; } - if(!storedMag.init(OBS_BUFFER_LENGTH)) { + + // find the maximum time delay for all potential sensors + uint16_t maxTimeDelay_ms = MAX(_frontend->_gpsDelay_ms , + MAX(_frontend->_hgtDelay_ms , + MAX(_frontend->_flowDelay_ms , + MAX(_frontend->_rngBcnDelay_ms , + MAX(_frontend->magDelay_ms , + (uint16_t)(dtEkfAvg*1000.0f) + ))))); + + // airspeed sensing can have large delays and should not be included if disabled + if (_ahrs->airspeed_sensor_enabled()) { + maxTimeDelay_ms = MAX(maxTimeDelay_ms , _frontend->tasDelay_ms); + } + + // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter + imu_buffer_length = (maxTimeDelay_ms / (uint16_t)(dtEkfAvg*1000.0f)) + 1; + + // set the observaton buffer length to handle the minimum time of arrival between observations in combination + // with the worst case delay from current time to ekf fusion time + // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter + uint16_t ekf_delay_ms = maxTimeDelay_ms + (int)(ceil((float)maxTimeDelay_ms * 0.5f)); + obs_buffer_length = (ekf_delay_ms / _frontend->sensorIntervalMin_ms) + 1; + + // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) + obs_buffer_length = MIN(obs_buffer_length,imu_buffer_length); + + if(!storedGPS.init(obs_buffer_length)) { return false; } - if(!storedBaro.init(OBS_BUFFER_LENGTH)) { + if(!storedMag.init(obs_buffer_length)) { return false; } - if(!storedTAS.init(OBS_BUFFER_LENGTH)) { + if(!storedBaro.init(obs_buffer_length)) { return false; } - if(!storedOF.init(OBS_BUFFER_LENGTH)) { + if(!storedTAS.init(obs_buffer_length)) { return false; } - // Note: the use of dual range finders potentially doubles the amount of to be stored - if(!storedRange.init(2*OBS_BUFFER_LENGTH)) { + if(!storedOF.init(obs_buffer_length)) { + return false; + } + // Note: the use of dual range finders potentially doubles the amount of data to be stored + if(!storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) { return false; } // Note: range beacon data is read one beacon at a time and can arrive at a high rate @@ -87,7 +114,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c if(!storedOutput.init(imu_buffer_length)) { return false; } - + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 buffers, IMU=%u , OBS=%u , dt=%6.4f",(unsigned)imu_buffer_length,(unsigned)obs_buffer_length,dtEkfAvg); return true; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index c0e71a1ccb..c86583a659 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -315,6 +315,7 @@ private: uint8_t imu_index; uint8_t core_index; uint8_t imu_buffer_length; + uint8_t obs_buffer_length; typedef float ftype; #if MATH_CHECK_INDEXES @@ -721,10 +722,6 @@ private: // initialise the quaternion covariances using rotation vector variances void initialiseQuatCovariances(Vector3f &rotVarVec); - // Length of FIFO buffers used for non-IMU sensor data. - // Must be larger than the time period defined by IMU_BUFFER_LENGTH - static const uint32_t OBS_BUFFER_LENGTH = 5; - // Variables bool statesInitialised; // boolean true when filter states have been initialised bool velHealth; // boolean true if velocity measurements have passed innovation consistency check