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

AP_NavEKF2: use parent class for intermediate static variables 

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-10-06 08:35:10 +11:00
parent 46628b4401
commit 9ad9fe42bc
4 changed files with 12 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -660,6 +660,8 @@ bool NavEKF2::InitialiseFilter(void)
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: 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();

2
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -780,6 +780,7 @@ void NavEKF2_core::fuseEulerYaw()
float measured_yaw;
float H_YAW[3];
Matrix3f Tbn_zeroYaw;
if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
float t2 = q0*q0;
@ -1037,6 +1038,7 @@ void NavEKF2_core::FuseDeclination(float declErr)
float t12 = 1.0f/t11;
float H_MAG[24];
H_MAG[16] = -magE*t5;
H_MAG[17] = magN*t5;

8
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -42,7 +42,8 @@ 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)
{
_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,8 +171,9 @@ 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(&processNoise[0], 0, sizeof(processNoise));
flowDataValid = false;
rangeDataToFuse = false;
Popt = 0.0f;
@ -545,6 +547,8 @@ void NavEKF2_core::UpdateFilter(bool predict)
#endif
hal.util->perf_begin(_perf_UpdateFilter);
fill_scratch_variables();
// TODO - in-flight restart method
//get starting time for update step

15
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -30,6 +30,7 @@
#include "AP_NavEKF2.h"
#include <stdio.h>
#include <AP_Math/vectorN.h>
#include <AP_NavEKF/AP_NavEKF_core_common.h>
#include <AP_NavEKF2/AP_NavEKF2_Buffer.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
@ -58,7 +59,7 @@
class AP_AHRS;
class NavEKF2_core
class NavEKF2_core : public NavEKF_core_common
{
public:
// Constructor
@ -336,7 +337,6 @@ private:
uint8_t core_index;
uint8_t imu_buffer_length;
typedef float ftype;
#if MATH_CHECK_INDEXES
typedef VectorN<ftype,2> Vector2;
typedef VectorN<ftype,3> Vector3;
@ -356,7 +356,6 @@ private:
typedef VectorN<ftype,24> Vector24;
typedef VectorN<ftype,25> Vector25;
typedef VectorN<ftype,31> Vector31;
typedef VectorN<ftype,28> Vector28;
typedef VectorN<VectorN<ftype,3>,3> Matrix3;
typedef VectorN<VectorN<ftype,24>,24> Matrix24;
typedef VectorN<VectorN<ftype,34>,50> Matrix34_50;
@ -379,7 +378,6 @@ private:
typedef ftype Vector23[23];
typedef ftype Vector24[24];
typedef ftype Vector25[25];
typedef ftype Vector28[28];
typedef ftype Matrix3[3][3];
typedef ftype Matrix24[24][24];
typedef ftype Matrix34_50[34][50];
@ -799,9 +797,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
@ -856,12 +851,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 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