Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: use parent class for intermediate static variables

Browse Source 
this makes the code faster as well as using less memory when both EK2
and EK3 are enabled
This commit is contained in:
Andrew Tridgell 2019-09-28 11:28:59 +10:00
parent da7dc71195
commit b1e0299a0f
4 changed files with 4 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -683,16 +683,6 @@ bool NavEKF3::InitialiseFilter(void)
return false;
}
// allocate common intermediate variable space
core_common = (void *)hal.util->malloc_type(NavEKF3_core::get_core_common_size(), AP_HAL::Util::MEM_FAST);
if (core_common == nullptr) {
_enable.set(0);
hal.util->free_type(core, sizeof(NavEKF3_core)*num_cores, AP_HAL::Util::MEM_FAST);
core = nullptr;
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF3: common allocation failed");
return false;
}
// Call constructors on all cores
for (uint8_t i = 0; i < num_cores; i++) {
new (&core[i]) NavEKF3_core(this);

5
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -496,11 +496,6 @@ private:
// time of last lane switch
uint32_t lastLaneSwitch_ms;
/*
common intermediate variables used by all cores
*/
void *core_common;
struct {
uint32_t last_function_call; // last time getLastYawYawResetAngle was called
bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise

18
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -20,13 +20,7 @@ NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) :
_perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_TerrainOffset")),
_perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow")),
_perf_FuseBodyOdom(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseBodyOdom")),
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)
frontend(_frontend)
{
_perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test0");
_perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test1");
@ -217,7 +211,9 @@ void NavEKF3_core::InitialiseVariables()
lastKnownPositionNE.zero();
prevTnb.zero();
memset(&P[0][0], 0, sizeof(P));
memset(common, 0, sizeof(*common));
memset(&KH[0][0], 0, sizeof(KH));
memset(&KHP[0][0], 0, sizeof(KHP));
memset(&nextP[0][0], 0, sizeof(nextP));
flowDataValid = false;
rangeDataToFuse = false;
Popt = 0.0f;
@ -573,12 +569,6 @@ void NavEKF3_core::CovarianceInit()
// Update Filter States - this should be called whenever new IMU data is available
void NavEKF3_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;

22
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -367,9 +367,6 @@ public:
// get timing statistics structure
void getTimingStatistics(struct ekf_timing &timing);
// 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
NavEKF3 *frontend;
@ -535,21 +532,6 @@ private:
uint8_t type; // type specifiying Euler rotation order used, 1 = 312 (ZXY), 2 = 321 (ZYX)
};
/*
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;
} *common;
// bias estimates for the IMUs that are enabled but not being used
// by this core.
struct {
@ -872,9 +854,6 @@ 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
@ -928,7 +907,6 @@ private:
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t lastSynthYawTime_ms; // time stamp when synthetic yaw measurement was last fused to maintain covariance health (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Matrix24 &nextP; // Predicted covariance matrix before addition of process noise to diagonals
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