From 78ac1340c8497c0f7e695f16f7d1b6663b045c01 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 11 Aug 2015 03:57:16 +1000 Subject: [PATCH] AP_NavEKF: Prevent false triggering of optical flow takeoff detection Now that we have a pre-arm check in place to detect bad lidar, the motion check is unnecessary and can false trigger for copters with flexible undercarriages or on uneven ground. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 2080c448cd..857c6d20f7 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -5288,18 +5288,7 @@ void NavEKF::readRangeFinder(void) void NavEKF::detectOptFlowTakeoff(void) { if (vehicleArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { - const AP_InertialSensor &ins = _ahrs->get_ins(); - Vector3f angRateVec; - Vector3f gyroBias; - getGyroBias(gyroBias); - bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1); - if (dual_ins) { - angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias; - } else { - angRateVec = ins.get_gyro() - gyroBias; - } - - takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rangeAtArming + 0.1f))); + takeOffDetected = (takeOffDetected || (rngMea > (rangeAtArming + 0.1f))); } }