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

AP_NavEKF: Prevent divide by zero casued by Tnb_flow.c.z = 0

This commit is contained in:
priseborough 2015-01-03 10:35:54 +11:00 committed by Andrew Tridgell
parent 1e3c23e5a3
commit 75201c8968
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -926,14 +926,14 @@ void NavEKF::SelectFlowFusion()
// update the time stamp // update the time stamp
prevFlowFusionTime_ms = imuSampleTime_ms; prevFlowFusionTime_ms = imuSampleTime_ms;
} else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode){ } else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode && tiltOK){
// 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 (((flowDataValid && flow_state.obsIndex == 2) || newDataRng) && !constPosMode) { } else if (((flowDataValid && flow_state.obsIndex == 2) || newDataRng) && !constPosMode && tiltOK) {
// 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;