From 5f3a20ea6ae533db728c5f24a18e8678b4ec2f78 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 12 Mar 2018 15:46:44 -0700 Subject: [PATCH] Plane: auto-disarm quadplane using LAND_DISARMDELAY --- ArduPlane/quadplane.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 6c18053f39a2b..7bd9a601fd129 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2157,11 +2157,23 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) */ void QuadPlane::check_land_complete(void) { + const uint32_t now = AP_HAL::millis(); + + if (poscontrol.state == QPOS_LAND_COMPLETE && + plane.arming.is_armed() && + plane.landing.get_disarm_delay() > 0 && + now - landing_detect.lower_limit_start_ms >= plane.landing.get_disarm_delay()*1000UL) { + // disarm after user-defined delay + if (plane.disarm_motors()) { + gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed"); + } + } + if (poscontrol.state != QPOS_LAND_FINAL) { // only apply to final landing phase return; } - const uint32_t now = AP_HAL::millis(); + bool might_be_landed = (landing_detect.lower_limit_start_ms != 0 && now - landing_detect.lower_limit_start_ms > 1000); if (!might_be_landed) { @@ -2190,7 +2202,6 @@ void QuadPlane::check_land_complete(void) landing_detect.land_start_ms = 0; // motors have been at zero for 5s, and we have had less than 0.3m // change in altitude for last 4s. We are landed. - plane.disarm_motors(); poscontrol.state = QPOS_LAND_COMPLETE; gcs().send_text(MAV_SEVERITY_INFO,"Land complete"); // reload target airspeed which could have been modified by the mission