mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF: Prevent divide by zero casued by Tnb_flow.c.z = 0
This commit is contained in:
parent
1e3c23e5a3
commit
75201c8968
|
@ -926,14 +926,14 @@ void NavEKF::SelectFlowFusion()
|
|||
// update the time stamp
|
||||
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
|
||||
FuseOptFlow();
|
||||
// increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle
|
||||
flow_state.obsIndex = 2;
|
||||
// indicate that flow fusion has been performed. This is used for load spreading.
|
||||
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
|
||||
if(newDataRng && useRngFinder()) {
|
||||
fuseRngData = true;
|
||||
|
|
Loading…
Reference in New Issue