AP_NavEKF2: Offset the fusion time horizon between multiple instances

Prevents frame over-runs due to simultaneous fusion of measurements on each instance.
The offset is only applied if less than 5msec available between frames
This commit is contained in:
Paul Riseborough 2015-11-06 12:04:20 +11:00 committed by Andrew Tridgell
parent 3eeff8dbc6
commit 2340e18fdc
4 changed files with 17 additions and 5 deletions

View File

@ -472,7 +472,8 @@ bool NavEKF2::InitialiseFilter(void)
num_cores = 0;
for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) {
core[num_cores++].setup_core(this, i);
core[num_cores].setup_core(this, i, num_cores);
num_cores++;
}
}
}

View File

@ -316,8 +316,9 @@ void NavEKF2_core::StoreIMU()
fifoIndexNow = 0;
}
storedIMU[fifoIndexNow] = imuDataNew;
// set the index required to access the oldest data
fifoIndexDelayed = fifoIndexNow + 1;
// set the index required to access the oldest data, applying an offset to the fusion time horizon that is used to
// prevent the same fusion operation being performed on the same frame across multiple EKF's
fifoIndexDelayed = fifoIndexNow + 1 + fusionHorizonOffset;
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
fifoIndexDelayed = 0;
}

View File

@ -55,10 +55,11 @@ NavEKF2_core::NavEKF2_core(void) :
}
// setup this core backend
void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index)
void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index)
{
frontend = _frontend;
imu_index = _imu_index;
core_index = _core_index;
_ahrs = frontend->_ahrs;
}
@ -70,6 +71,10 @@ void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index)
// Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
void NavEKF2_core::InitialiseVariables()
{
// Offset the fusion horizon if necessary to prevent frame over-runs
if (dtIMUavg < 0.005) {
fusionHorizonOffset = 2*core_index;
}
// initialise time stamps
imuSampleTime_ms = hal.scheduler->millis();
lastHealthyMagTime_ms = imuSampleTime_ms;

View File

@ -47,6 +47,9 @@
* Samples*delta_time must be > max sensor delay
*/
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
// Note that if using more than 2 instances of the EKF, as set by EK2_IMU_MASK, this delay should be increased by 2 samples
// for each additional instance to allow for the need to offset the fusion time horizon for each instance to avoid simultaneous fusion
// of measurements by each instance
#define IMU_BUFFER_LENGTH 104 // maximum 260 msec delay at 400 Hz
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
#define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz
@ -65,7 +68,7 @@ public:
NavEKF2_core(void);
// setup this core backend
void setup_core(NavEKF2 *_frontend, uint8_t _imu_index);
void setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index);
// Initialise the states from accelerometer and magnetometer data (if present)
// This method can only be used when the vehicle is static
@ -266,6 +269,7 @@ private:
// Reference to the global EKF frontend for parameters
NavEKF2 *frontend;
uint8_t imu_index;
uint8_t core_index;
typedef float ftype;
#if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1)
@ -777,6 +781,7 @@ private:
uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
Quaternion prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
uint8_t fusionHorizonOffset; // number of IMU samples that the fusion time horizon has been shifted forward to prevent multiple EKF instances fusing data at the same time
// variables used to calulate a vertical velocity that is kinematically consistent with the verical position
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.