mirror of https://github.com/ArduPilot/ardupilot
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:
parent
3eeff8dbc6
commit
2340e18fdc
|
@ -472,7 +472,8 @@ bool NavEKF2::InitialiseFilter(void)
|
||||||
num_cores = 0;
|
num_cores = 0;
|
||||||
for (uint8_t i=0; i<7; i++) {
|
for (uint8_t i=0; i<7; i++) {
|
||||||
if (_imuMask & (1U<<i)) {
|
if (_imuMask & (1U<<i)) {
|
||||||
core[num_cores++].setup_core(this, i);
|
core[num_cores].setup_core(this, i, num_cores);
|
||||||
|
num_cores++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -316,8 +316,9 @@ void NavEKF2_core::StoreIMU()
|
||||||
fifoIndexNow = 0;
|
fifoIndexNow = 0;
|
||||||
}
|
}
|
||||||
storedIMU[fifoIndexNow] = imuDataNew;
|
storedIMU[fifoIndexNow] = imuDataNew;
|
||||||
// set the index required to access the oldest data
|
// set the index required to access the oldest data, applying an offset to the fusion time horizon that is used to
|
||||||
fifoIndexDelayed = fifoIndexNow + 1;
|
// prevent the same fusion operation being performed on the same frame across multiple EKF's
|
||||||
|
fifoIndexDelayed = fifoIndexNow + 1 + fusionHorizonOffset;
|
||||||
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
|
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
|
||||||
fifoIndexDelayed = 0;
|
fifoIndexDelayed = 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -55,10 +55,11 @@ NavEKF2_core::NavEKF2_core(void) :
|
||||||
}
|
}
|
||||||
|
|
||||||
// setup this core backend
|
// 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;
|
frontend = _frontend;
|
||||||
imu_index = _imu_index;
|
imu_index = _imu_index;
|
||||||
|
core_index = _core_index;
|
||||||
_ahrs = frontend->_ahrs;
|
_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.
|
// 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()
|
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
|
// initialise time stamps
|
||||||
imuSampleTime_ms = hal.scheduler->millis();
|
imuSampleTime_ms = hal.scheduler->millis();
|
||||||
lastHealthyMagTime_ms = imuSampleTime_ms;
|
lastHealthyMagTime_ms = imuSampleTime_ms;
|
||||||
|
|
|
@ -47,6 +47,9 @@
|
||||||
* Samples*delta_time must be > max sensor delay
|
* Samples*delta_time must be > max sensor delay
|
||||||
*/
|
*/
|
||||||
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
|
#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
|
#define IMU_BUFFER_LENGTH 104 // maximum 260 msec delay at 400 Hz
|
||||||
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
|
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
|
||||||
#define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz
|
#define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz
|
||||||
|
@ -65,7 +68,7 @@ public:
|
||||||
NavEKF2_core(void);
|
NavEKF2_core(void);
|
||||||
|
|
||||||
// setup this core backend
|
// 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)
|
// Initialise the states from accelerometer and magnetometer data (if present)
|
||||||
// This method can only be used when the vehicle is static
|
// This method can only be used when the vehicle is static
|
||||||
|
@ -266,6 +269,7 @@ private:
|
||||||
// Reference to the global EKF frontend for parameters
|
// Reference to the global EKF frontend for parameters
|
||||||
NavEKF2 *frontend;
|
NavEKF2 *frontend;
|
||||||
uint8_t imu_index;
|
uint8_t imu_index;
|
||||||
|
uint8_t core_index;
|
||||||
|
|
||||||
typedef float ftype;
|
typedef float ftype;
|
||||||
#if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1)
|
#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
|
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
|
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
|
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
|
// 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.
|
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
|
||||||
|
|
Loading…
Reference in New Issue