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

AP_NavEKF3: added inter-EKF scheduling cooperation 

this changes the stragegy for load levelling between EKF cores so it
works between EK2 and EK3, and with future estimators as well.

It allows us to run EK3 and EK2 at the same time with good scheduling
performance
This commit is contained in:
Andrew Tridgell 2017-04-29 10:48:49 +10:00
parent 2de8777669
commit a04aff7a7d
4 changed files with 20 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -607,9 +607,16 @@ bool NavEKF3::InitialiseFilter(void)
if (_enable == 0) {
return false;
}
const AP_InertialSensor &ins = _ahrs->get_ins();
imuSampleTime_us = AP_HAL::micros64();
// remember expected frame time
_frameTimeUsec = 1e6 / ins.get_sample_rate();
// expected number of IMU frames per prediction
_framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));
if (core == nullptr) {
// see if we will be doing logging
@ -619,7 +626,6 @@ bool NavEKF3::InitialiseFilter(void)
}
// don't run multiple filters for 1 IMU
const AP_InertialSensor &ins = _ahrs->get_ins();
uint8_t mask = (1U<<ins.get_accel_count())-1;
_imuMask.set(_imuMask.get() & mask);
@ -705,10 +711,12 @@ void NavEKF3::UpdateFilter(void)
bool statePredictEnabled[num_cores];
for (uint8_t i=0; i<num_cores; i++) {
// if the previous core has only recently finished a new state prediction cycle, then
// don't start a new cycle to allow time for fusion operations to complete if the update
// rate is higher than 200Hz
if ((i > 0) && (core[i-1].getFramesSincePredict() < 2) && (ins.get_sample_rate() > 200)) {
// if we have not overrun by more than 3 IMU frames, and we
// have already used more than 1/3 of the CPU budget for this
// loop then suppress the prediction step. This allows
// multiple EKF instances to cooperate on scheduling
if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
(AP_HAL::micros() - ins.get_last_update_usec()) > _frameTimeUsec/3) {
statePredictEnabled[i] = false;
} else {
statePredictEnabled[i] = true;

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -339,6 +339,9 @@ private:
AP_Baro &_baro;
const RangeFinder &_rng;
uint32_t _frameTimeUsec; // time per IMU frame
uint8_t _framesPerPrediction; // expected number of IMU frames per prediction
// EKF Mavlink Tuneable Parameters
AP_Int8 _enable; // zero to disable EKF3
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -352,7 +352,8 @@ void NavEKF3_core::readIMUData()
* then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more
* than twice the target time has lapsed.
*/
if ((imuDataDownSampledNew.delAngDT >= EKF_TARGET_DT && startPredictEnabled) || (imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) {
if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5)) && startPredictEnabled) ||
(imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) {
// convert the accumulated quaternion to an equivalent delta angle
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -59,8 +59,8 @@
#define ACCEL_BIAS_LIM_SCALER 0.2f
// target update time for the EKF in msec and sec
#define EKF_TARGET_DT_MS 10.0f
#define EKF_TARGET_DT 0.01f
#define EKF_TARGET_DT_MS 10.0
#define EKF_TARGET_DT 0.01
class AP_AHRS;