From 0aa1ae048dd3acab4d80da865f0d617366382839 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 22 Apr 2016 20:20:06 +1000 Subject: [PATCH] Plane: fixed is_flying() for VTOL flight otherwise we may disarm mid-flight! --- ArduPlane/is_flying.cpp | 3 +++ ArduPlane/quadplane.cpp | 9 +++++++++ ArduPlane/quadplane.h | 3 +++ 3 files changed, 15 insertions(+) diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index c3bc964953..63c2651307 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -170,6 +170,9 @@ void Plane::update_is_flying_5Hz(void) bool Plane::is_flying(void) { if (hal.util->get_soft_armed()) { + if (quadplane.is_flying_vtol()) { + return true; + } // when armed, assume we're flying unless we probably aren't return (isFlyingProbability >= 0.1f); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 7be9cadcf5..c767e700b5 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -553,6 +553,15 @@ bool QuadPlane::should_relax(void) return relax_loiter; } +// see if we are flying in vtol +bool QuadPlane::is_flying_vtol(void) +{ + if (in_vtol_mode() && millis() - motors_lower_limit_start_ms > 5000) { + return true; + } + return false; +} + /* smooth out descent rate for landing to prevent a jerk as we get to land_final_alt. diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index d60d82653b..4961fd2ef5 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -62,6 +62,9 @@ public: // return desired forward throttle percentage int8_t forward_throttle_pct(void); float get_weathervane_yaw_rate_cds(void); + + // see if we are flying from vtol point of view + bool is_flying_vtol(void); struct PACKED log_QControl_Tuning { LOG_PACKET_HEADER;