From f1dfa282dc69dc10755e9fcefdfa667fb1dfe811 Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 1 Jan 2015 09:39:19 +1100 Subject: [PATCH] AP_NavEKF: Consolidate constant velocity mode decision logic The decision to switch to constant velocity mode during optical flow operation and te decision to switch back were previously being made in two different places in code. Both decisions are now made in the one place which makes the code easier to follow and maintain. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index b3977c0dbd..e48b7538dc 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -909,6 +909,11 @@ void NavEKF::SelectFlowFusion() // if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in positon hold mode if (!flowDataValid && !constPosMode && PV_AidingMode == RELATIVE) { constVelMode = true; + constPosMode = false; // always clear constant position mode if constant velocity mode is active + } else if (flowDataValid) { + // clear the constant velocity mode now we have good data + constVelMode = false; + lastConstVelMode = false; } // Apply tilt check bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin); @@ -4094,9 +4099,6 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V flowRadXYcomp[1] = flowStates[0]*flowRadXY[1] + omegaAcrossFlowTime.y; // set flag that will trigger observations newDataFlow = true; - // clear the constant velocity mode now we have good data - constVelMode = false; - lastConstVelMode = false; flowValidMeaTime_ms = imuSampleTime_ms; } else { newDataFlow = false;