From c4b7a1c41a1b9f501277abaa36346acd988ea9b1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 18 Aug 2020 11:46:57 +0900 Subject: [PATCH] AP_NavEKF3: flowDataToFuse moved to local variable This variable is never used outside the SelectFlowFusion() method This variable is always updated at the top of the function meaning subsequent calls to the function will always overwrite its previous value --- libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 4 +--- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1 - 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 6e73189101..26839a536a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -31,7 +31,7 @@ void NavEKF3_core::SelectFlowFusion() } // Check for data at the fusion time horizon - flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); + const bool flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); // Perform Data Checks // Check if the optical flow data is still valid @@ -63,8 +63,6 @@ void NavEKF3_core::SelectFlowFusion() // Fuse the optical flow X and Y axis data into the main filter sequentially FuseOptFlow(); } - // reset flag to indicate that no new flow data is available for fusion - flowDataToFuse = false; } // stop the performance timer diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index a4c5550d6f..c831b84955 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1195,7 +1195,6 @@ private: of_elements ofDataNew; // OF data at the current time horizon of_elements ofDataDelayed; // OF data at the fusion time horizon uint8_t ofStoreIndex; // OF data storage index - bool flowDataToFuse; // true when optical flow data has is ready for fusion 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)