diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 12106d6647..fcd9fbfb12 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -102,8 +102,6 @@ void NavEKF2_core::setAidingMode() // We have transitioned either into or out of aiding // zero stored velocities used to do dead-reckoning heldVelNE.zero(); - // reset the flag that indicates takeoff for use by optical flow navigation - takeOffDetected = false; // set various useage modes based on the condition when we start aiding. These are then held until aiding is stopped. if (!isAiding) { // We have ceased aiding diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index f6634fd1d5..70db557346 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -401,7 +401,8 @@ void NavEKF2_core::setTouchdownExpected(bool val) // Detect takeoff for optical flow navigation void NavEKF2_core::detectOptFlowTakeoff(void) { - if (motorsArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { + if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { + // we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff const AP_InertialSensor &ins = _ahrs->get_ins(); Vector3f angRateVec; Vector3f gyroBias; @@ -414,6 +415,9 @@ void NavEKF2_core::detectOptFlowTakeoff(void) } takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rngAtStartOfFlight + 0.1f))); + } else if (onGround) { + // we are confidently on the ground so set the takeoff detected status to false + takeOffDetected = false; } }