From 0cebde0632b6e81bd760af4ecea815f50b12e57e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 4 Jul 2020 12:39:08 +0900 Subject: [PATCH] AP_NavEKF3: minor restructure of how fusePosData and fuseVelData are set this non-functional change slightly reduces the number of places we set these variables --- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 48cab13345..5f15d6640b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -407,6 +407,10 @@ void NavEKF3_core::SelectVelPosFusion() // get data that has now fallen behind the fusion time horizon gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms); + // initialise all possible data we may fuse + fusePosData = false; + fuseVelData = false; + // Determine if we need to fuse position and velocity data on this time step if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3)) { @@ -433,7 +437,6 @@ void NavEKF3_core::SelectVelPosFusion() // use external nav system for position extNavUsedForPos = true; activeHgtSource = HGT_SOURCE_EXTNAV; - fuseVelData = false; fuseHgtData = true; fusePosData = true; @@ -443,9 +446,6 @@ void NavEKF3_core::SelectVelPosFusion() velPosObs[3] = extNavDataDelayed.pos.x; velPosObs[4] = extNavDataDelayed.pos.y; velPosObs[5] = extNavDataDelayed.pos.z; - } else { - fuseVelData = false; - fusePosData = false; } // fuse external navigation velocity data if available