From 5d09c78f348dde98bbc831112172dce2b9b247a4 Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 7 Oct 2016 20:35:55 +1100 Subject: [PATCH] AP_NavEKF2: Always perform optical flow takeoff check when receiving data A specialised takeoff check is now always performed when we receive new flow data as the default behaviour is to try and use flow data whenever it is received, rather than limit its use to a use to a flow-only mode of operation that had to be selected via user parameter. --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index fbebe931b8..0b319761b3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -127,11 +127,10 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa delAngBodyOF.zero(); delTimeOF = 0.0f; } - // check for takeoff if relying on optical flow and zero measurements until takeoff detected - // if we haven't taken off - constrain position and velocity states - if (frontend->_fusionModeGPS == 3) { - detectOptFlowTakeoff(); - } + // by definition if this function is called, then flow measurements have been provided so we + // need to run the optical flow takeoff detection + detectOptFlowTakeoff(); + // calculate rotation matrices at mid sample time for flow observations stateStruct.quat.rotation_matrix(Tbn_flow); // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)