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

AP_NavEKF3: make time-horizon OF data a local variable

Browse Source 
Saves 40 bytes of RAM per core on stm32
This commit is contained in:
Peter Barker 2021-05-28 09:26:57 +10:00 committed by Andrew Tridgell
parent 0dbd05505b
commit 15658362b5
2 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -25,6 +25,8 @@ void NavEKF3_core::SelectFlowFusion()
optFlowFusionDelayed = false; optFlowFusionDelayed = false;
} }
of_elements ofDataDelayed; // OF data at the fusion time horizon
// Check for data at the fusion time horizon // Check for data at the fusion time horizon
const bool flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); const bool flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
@ -47,7 +49,7 @@ void NavEKF3_core::SelectFlowFusion()
// if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height // if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height
if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) { if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) {
// Estimate the terrain offset (runs a one state EKF) // Estimate the terrain offset (runs a one state EKF)
EstimateTerrainOffset(); EstimateTerrainOffset(ofDataDelayed);
} }
// Fuse optical flow data into the main filter // Fuse optical flow data into the main filter
@ -56,7 +58,7 @@ void NavEKF3_core::SelectFlowFusion()
// Set the flow noise used by the fusion processes // Set the flow noise used by the fusion processes
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f)); R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
// Fuse the optical flow X and Y axis data into the main filter sequentially // Fuse the optical flow X and Y axis data into the main filter sequentially
FuseOptFlow(); FuseOptFlow(ofDataDelayed);
} }
} }
} }
@ -66,7 +68,7 @@ Estimation of terrain offset using a single state EKF
The filter can fuse motion compensated optical flow rates and range finder measurements The filter can fuse motion compensated optical flow rates and range finder measurements
Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scripts/Terrain%20Estimator Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scripts/Terrain%20Estimator
*/ */
void NavEKF3_core::EstimateTerrainOffset() void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
{ {
// horizontal velocity squared // horizontal velocity squared
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
@ -264,7 +266,7 @@ void NavEKF3_core::EstimateTerrainOffset()
* https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
* Requires a valid terrain height estimate. * Requires a valid terrain height estimate.
*/ */
void NavEKF3_core::FuseOptFlow() void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
{ {
Vector24 H_LOS; Vector24 H_LOS;
Vector3f relVelSensor; Vector3f relVelSensor;

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -795,10 +795,10 @@ private:
void SelectBodyOdomFusion(); void SelectBodyOdomFusion();
// Estimate terrain offset using a single state EKF // Estimate terrain offset using a single state EKF
void EstimateTerrainOffset(); void EstimateTerrainOffset(const of_elements &ofDataDelayed);
// fuse optical flow measurements into the main filter // fuse optical flow measurements into the main filter
void FuseOptFlow(); void FuseOptFlow(const of_elements &ofDataDelayed);
// Control filter mode changes // Control filter mode changes
void controlFilterModes(); void controlFilterModes();
@ -1145,7 +1145,6 @@ private:
// variables added for optical flow fusion // variables added for optical flow fusion
EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer
of_elements ofDataNew; // OF data at the current time horizon of_elements ofDataNew; // OF data at the current time horizon
of_elements ofDataDelayed; // OF data at the fusion time horizon
bool flowDataValid; // true while optical flow data is still fresh bool flowDataValid; // true while optical flow data is still fresh
Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec) uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)