aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: moved intermediate variables to common memory

Browse Source 
this moves intermediate variables from being per-core to being common
between cores. This saves memory on systems with more than one core by
avoiding allocating this memory on every core.

This is an alternative to #11717 which moves memory onto the stack. It
doesn't save as much memory as #11717, but avoids creating large stack
frames
This commit is contained in:
Andrew Tridgell 2019-09-21 10:41:46 +10:00
parent 9b746b89db
commit 0041874826
4 changed files with 75 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -660,16 +660,27 @@ bool NavEKF2::InitialiseFilter(void)
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
return false;
}
// allocate common intermediate variable space
core_common = (struct CoreCommon *)hal.util->malloc_type(NavEKF2_core::get_core_common_size(), AP_HAL::Util::MEM_FAST);
if (core_common == nullptr) {
_enable.set(0);
hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
core = nullptr;
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: common allocation failed");
return false;
}
//Call Constructors on all cores
for (uint8_t i = 0; i < num_cores; i++) {
//Call Constructors
new (&core[i]) NavEKF2_core();
new (&core[i]) NavEKF2_core(this);
}
// set the IMU index for the cores
num_cores = 0;
for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) {
if(!core[num_cores].setup_core(this, i, num_cores)) {
if(!core[num_cores].setup_core(i, num_cores)) {
return false;
}
num_cores++;

5
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -492,6 +492,11 @@ private:
// time of last lane switch
uint32_t lastLaneSwitch_ms;
/*
common intermediate variables used by all cores
*/
void *core_common;
// update the yaw reset data to capture changes due to a lane switch
// new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary

28
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -20,7 +20,7 @@ extern const AP_HAL::HAL& hal;
#define GYRO_BIAS_LIMIT 0.5f
// constructor
NavEKF2_core::NavEKF2_core(void) :
NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
_perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")),
_perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_CovariancePrediction")),
_perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseVelPosNED")),
@ -28,7 +28,19 @@ NavEKF2_core::NavEKF2_core(void) :
_perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseAirspeed")),
_perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseSideslip")),
_perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_TerrainOffset")),
_perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseOptFlow"))
_perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseOptFlow")),
frontend(_frontend),
// setup the intermediate variables shared by all cores (to save memory)
common((struct core_common *)_frontend->core_common),
Kfusion(common->Kfusion),
KH(common->KH),
KHP(common->KHP),
nextP(common->nextP),
processNoise(common->processNoise),
SF(common->SF),
SG(common->SG),
SQ(common->SQ),
SPP(common->SPP)
{
_perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test0");
_perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test1");
@ -43,9 +55,8 @@ NavEKF2_core::NavEKF2_core(void) :
}
// setup this core backend
bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index)
bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
{
frontend = _frontend;
imu_index = _imu_index;
gyro_index_active = _imu_index;
accel_index_active = _imu_index;
@ -156,8 +167,7 @@ void NavEKF2_core::InitialiseVariables()
lastKnownPositionNE.zero();
prevTnb.zero();
memset(&P[0][0], 0, sizeof(P));
memset(&nextP[0][0], 0, sizeof(nextP));
memset(&processNoise[0], 0, sizeof(processNoise));
memset(common, 0, sizeof(*common));
flowDataValid = false;
rangeDataToFuse = false;
Popt = 0.0f;
@ -515,6 +525,12 @@ void NavEKF2_core::CovarianceInit()
// Update Filter States - this should be called whenever new IMU data is available
void NavEKF2_core::UpdateFilter(bool predict)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// fill the common variables with NaN, so we catch any cases in
// SITL where they are used without initialisation
fill_nanf((float *)common, sizeof(*common)/sizeof(float));
#endif
// Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started
startPredictEnabled = predict;

45
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -62,10 +62,10 @@ class NavEKF2_core
{
public:
// Constructor
NavEKF2_core(void);
NavEKF2_core(NavEKF2 *_frontend);
// setup this core backend
bool setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index);
bool setup_core(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
@ -327,6 +327,9 @@ public:
// return true when external nav data is also being used as a yaw observation
bool isExtNavUsedForYaw(void);
// return size of core_common for use in frontend allocation
static uint32_t get_core_common_size(void) { return sizeof(core_common); }
private:
// Reference to the global EKF frontend for parameters
NavEKF2 *frontend;
@ -482,6 +485,26 @@ private:
bool posReset; // true when the position measurement has been reset
};
/*
common intermediate variables used by all cores. These save a
lot memory by avoiding allocating these arrays on every core
Having these as stack variables would save even more memory, but
would give us very high stack usage in some functions, which
poses a risk of stack overflow until we have infrastructure in
place to calculate maximum stack usage using static analysis.
*/
struct core_common {
Vector28 Kfusion;
Matrix24 KH;
Matrix24 KHP;
Matrix24 nextP;
Vector24 processNoise;
Vector25 SF;
Vector5 SG;
Vector8 SQ;
Vector23 SPP;
} *common;
// bias estimates for the IMUs that are enabled but not being used
// by this core.
struct {
@ -799,9 +822,9 @@ private:
bool badIMUdata; // boolean true if the bad IMU data is detected
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
Matrix24 KHP; // intermediate result used for covariance updates
Vector28 &Kfusion; // Kalman gain vector
Matrix24 &KH; // intermediate result used for covariance updates
Matrix24 &KHP; // intermediate result used for covariance updates
Matrix24 P; // covariance matrix
imu_ring_buffer_t<imu_elements> storedIMU; // IMU data buffer
obs_ring_buffer_t<gps_elements> storedGPS; // GPS data buffer
@ -856,12 +879,12 @@ private:
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t lastYawTime_ms; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals
Vector24 processNoise; // process noise added to diagonals of predicted covariance matrix
Vector25 SF; // intermediate variables used to calculate predicted covariance matrix
Vector5 SG; // intermediate variables used to calculate predicted covariance matrix
Vector8 SQ; // intermediate variables used to calculate predicted covariance matrix
Vector23 SPP; // intermediate variables used to calculate predicted covariance matrix
Matrix24 &nextP; // Predicted covariance matrix before addition of process noise to diagonals
Vector24 &processNoise; // process noise added to diagonals of predicted covariance matrix
Vector25 &SF; // intermediate variables used to calculate predicted covariance matrix
Vector5 &SG; // intermediate variables used to calculate predicted covariance matrix
Vector8 &SQ; // intermediate variables used to calculate predicted covariance matrix
Vector23 &SPP; // intermediate variables used to calculate predicted covariance matrix
Vector2f lastKnownPositionNE; // last known position
uint32_t lastDecayTime_ms; // time of last decay of GPS position offset
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold