Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
priseborough 2016-12-18 09:02:38 +11:00 committed by Randy Mackay
parent 2cc48f78a7
commit ed5039823f
6 changed files with 74 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

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

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

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

27
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

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

8
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

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

65
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -8,8 +8,7 @@
#include "AP_NavEKF3_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <stdio.h>
#include <GCS_MAVLink/GCS.h>
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;
}

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

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