From 8a56ce49be48a0cce694e25229465b8c8b186c72 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 17 Aug 2020 21:03:19 +0900 Subject: [PATCH] AP_NavEKF3: optical flow fusion checks source --- libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 367319b777..c56c5d2775 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -52,7 +52,7 @@ void NavEKF3_core::SelectFlowFusion() // Fuse optical flow data into the main filter if (flowDataToFuse && tiltOK) { - if (frontend->_flowUse == FLOW_USE_NAV) { + if ((frontend->_flowUse == FLOW_USE_NAV) && (frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::OPTFLOW)) { // 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