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;
}
of_elements ofDataDelayed; // OF data at the fusion time horizon
// Check for data at the fusion time horizon
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 (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) {
// Estimate the terrain offset (runs a one state EKF)
EstimateTerrainOffset();
EstimateTerrainOffset(ofDataDelayed);
}
// 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
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
// 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
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
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
* Requires a valid terrain height estimate.
*/
void NavEKF3_core::FuseOptFlow()
void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
{
Vector24 H_LOS;
Vector3f relVelSensor;

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

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