Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: added inter-EKF scheduling cooperation

Browse Source 
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 e62fdad12c
commit 2de8777669
4 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -609,9 +609,16 @@ bool NavEKF2::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));
// see if we will be doing logging
DataFlash_Class *dataflash = DataFlash_Class::instance();
if (dataflash != nullptr) {
@ -621,7 +628,6 @@ bool NavEKF2::InitialiseFilter(void)
if (core == nullptr) {
// 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);
@ -690,10 +696,12 @@ void NavEKF2::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_NavEKF2/AP_NavEKF2.h
View File

@ -322,6 +322,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 EKF2
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s

3
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -327,7 +327,8 @@ void NavEKF2_core::readIMUData()
// If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data
// to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed
if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {
if ((dtIMUavg*(float)framesSincePredict >= (EKF_TARGET_DT-(dtIMUavg*0.5)) &&
startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {
// convert the accumulated quaternion to an equivalent delta angle
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);

5
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -47,6 +47,9 @@
#define HGT_SOURCE_GPS 2
#define HGT_SOURCE_BCN 3
// target EKF update time step
#define EKF_TARGET_DT 0.01f
class AP_AHRS;
class NavEKF2_core
@ -728,8 +731,6 @@ private:
bool badMagYaw; // boolean true if the magnetometer is declared to be producing bad data
bool badIMUdata; // boolean true if the bad IMU data is detected
const float EKF_TARGET_DT = 0.01f; // target EKF update time step
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Vector28 Kfusion; // Kalman gain vector
Matrix24 KH; // intermediate result used for covariance updates