From 39e1eb238deff24af8d5b95422be7cd7d53502b3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 27 Dec 2020 18:55:42 +1100 Subject: [PATCH] Plane: make detection of impending takeoff more likely --- ArduPlane/servos.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 6a065bc2dd..0d6cfda1fe 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -556,8 +556,8 @@ void Plane::set_servos_controlled(void) // let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); - if (!is_flying() && arming.is_armed()) { - bool throw_detected = sq(ahrs.get_accel_ef().x) + sq(ahrs.get_accel_ef().y) > sq(g.takeoff_throttle_min_accel); + if (arming.is_armed()) { + bool throw_detected = ahrs.get_accel().x - GRAVITY_MSS * ahrs.sin_pitch() > GRAVITY_MSS; bool throttle_up_detected = throttle > aparm.throttle_cruise; if (throw_detected || throttle_up_detected) { plane.ahrs.setTakeoffExpected(true);