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

AP_NavEKF: Prevent flow scale factor updating on ground

This commit is contained in:
priseborough 2014-11-14 20:59:26 +11:00 committed by Andrew Tridgell
parent 083e22966c
commit 599e53f3f2
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -969,14 +969,14 @@ void NavEKF::SelectFlowFusion()
// update the time stamp // update the time stamp
prevFlowFusionTime_ms = imuSampleTime_ms; prevFlowFusionTime_ms = imuSampleTime_ms;
} else if (flow_state.obsIndex == 1 && !delayFusion){ } else if (flow_state.obsIndex == 1 && !delayFusion && !staticMode){
// Fuse the optical flow Y axis data into the main filter // Fuse the optical flow Y axis data into the main filter
FuseOptFlow(); FuseOptFlow();
// increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle // increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle
flow_state.obsIndex = 2; flow_state.obsIndex = 2;
// indicate that flow fusion has been performed. This is used for load spreading. // indicate that flow fusion has been performed. This is used for load spreading.
flowFusePerformed = true; flowFusePerformed = true;
} else if (flow_state.obsIndex == 2 || newDataRng) { } else if ((flow_state.obsIndex == 2 || newDataRng) && !staticMode) {
// enable fusion of range data if available and permitted // enable fusion of range data if available and permitted
if(newDataRng && useRngFinder()) { if(newDataRng && useRngFinder()) {
fuseRngData = true; fuseRngData = true;
@ -2595,7 +2595,7 @@ void NavEKF::RunAuxiliaryEKF()
inhibitGndState = false; inhibitGndState = false;
} }
// Don't update focal length offset if there is no range finder or not using GPS, or we are not flying fast enough to generate a useful LOS rate // Don't update focal length offset if there is no range finder or not using GPS, or we are not flying fast enough to generate a useful LOS rate
if (!fuseRngData || gpsInhibitMode == 2 || losRateSq < 0.01f) { if (!fuseRngData || gpsInhibitMode == 2 || losRateSq < 0.01f || (flowStates[1] - state.position[2]) < 3.0f) {
fScaleInhibit = true; fScaleInhibit = true;
} else { } else {
fScaleInhibit = false; fScaleInhibit = false;