Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues 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 676ab60424
commit a48d97c307
4 changed files with 66 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -661,6 +661,16 @@ bool NavEKF2::InitialiseFilter(void)
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++) {
new (&core[i]) NavEKF2_core(this);

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

23
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -43,7 +43,18 @@ NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
_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")),
frontend(_frontend)
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");
@ -170,9 +181,7 @@ void NavEKF2_core::InitialiseVariables()
lastKnownPositionNE.zero();
prevTnb.zero();
memset(&P[0][0], 0, sizeof(P));
memset(&KH[0][0], 0, sizeof(KH));
memset(&KHP[0][0], 0, sizeof(KHP));
memset(&nextP[0][0], 0, sizeof(nextP));
memset(common, 0, sizeof(*common));
flowDataValid = false;
rangeDataToFuse = false;
Popt = 0.0f;
@ -530,6 +539,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;

32
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -328,6 +328,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;
@ -480,6 +483,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 {
@ -797,6 +820,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
Matrix24 P; // covariance matrix
imu_ring_buffer_t<imu_elements> storedIMU; // IMU data buffer
obs_ring_buffer_t<gps_elements> storedGPS; // GPS data buffer
@ -851,6 +877,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
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