From 073b8e7c43bbffbb00ab685adb71d0fad178006b Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 11 Jan 2015 19:26:48 +1100 Subject: [PATCH] AP_NavEKF: Always explicitly set required const pos or vel mode when arming This additional explicit setting of the constPosMode and constVelMode reduces the likelihood of logic errors being introduced in the future as it places the intended setting of these parameters at arming in the one place. the constVelmode and constPosMode only have one set of conditions each that can trigger these modes in flight, so if these modes are true after arming it will be clear that it was the in-flight condition that triggered. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 92b989355d..2f8fa84b44 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4579,19 +4579,26 @@ void NavEKF::performArmingChecks() PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states posTimeout = true; velTimeout = true; + constPosMode = false; + constVelMode = false; } else { PV_AidingMode = AID_NONE; // we don't have optical flow data and will only be able to estimate orientation and height posTimeout = true; velTimeout = true; + constPosMode = true; + constVelMode = false; // always clear constant velocity mode if constant position mode is active } } else { // arming when GPS useage is allowed if (gpsNotAvailable) { PV_AidingMode = AID_NONE; // we don't have have GPS data and will only be able to estimate orientation and height posTimeout = true; velTimeout = true; + constPosMode = true; + constVelMode = false; // always clear constant velocity mode if constant position mode is active } else { PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states constPosMode = false; + constVelMode = false; } } // Reset filter positon, height and velocity states on arming or disarming