From 7751352a860d7fc9c6afb061dde0a1798b41b255 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 21 Aug 2019 16:01:17 +1000
Subject: [PATCH] Plane: implement VTOL landing for AFS termination

this allows for vertical landing as an AFS_TERM_ACTION
---
 ArduPlane/afs_plane.cpp | 6 ++++++
 ArduPlane/failsafe.cpp  | 4 +++-
 ArduPlane/quadplane.cpp | 6 ++++--
 ArduPlane/servos.cpp    | 4 +++-
 4 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp
index 8fbee726a3..cf2a6f55a4 100644
--- a/ArduPlane/afs_plane.cpp
+++ b/ArduPlane/afs_plane.cpp
@@ -16,6 +16,12 @@ AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission) :
  */
 void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
 {
+    if (plane.quadplane.available() && _terminate_action == TERMINATE_ACTION_LAND) {
+        // perform a VTOL landing
+        plane.set_mode(plane.mode_qland, MODE_REASON_FENCE_BREACH);
+        return;
+    }
+
     plane.g2.servo_channels.disable_passthrough(true);
     
     if (_terminate_action == TERMINATE_ACTION_LAND) {
diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp
index 9ff449ac9a..d89e32e1dd 100644
--- a/ArduPlane/failsafe.cpp
+++ b/ArduPlane/failsafe.cpp
@@ -86,7 +86,9 @@ void Plane::failsafe_check(void)
 #if ADVANCED_FAILSAFE == ENABLED
         if (afs.should_crash_vehicle()) {
             afs.terminate_vehicle();
-            return;
+            if (!afs.terminating_vehicle_via_landing()) {
+                return;
+            }
         }
 #endif
 
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 2b964d0153..87a52880a4 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -1653,7 +1653,7 @@ void QuadPlane::update(void)
     }
 
 #if ADVANCED_FAILSAFE == ENABLED
-    if (plane.afs.should_crash_vehicle()) {
+    if (plane.afs.should_crash_vehicle() && !plane.afs.terminating_vehicle_via_landing()) {
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
         motors->output();
         return;
@@ -1845,7 +1845,9 @@ void QuadPlane::motors_output(bool run_rate_controller)
     }
 
 #if ADVANCED_FAILSAFE == ENABLED
-    if (!hal.util->get_soft_armed() || plane.afs.should_crash_vehicle() || SRV_Channels::get_emergency_stop()) {
+    if (!hal.util->get_soft_armed() ||
+        (plane.afs.should_crash_vehicle() && !plane.afs.terminating_vehicle_via_landing()) ||
+         SRV_Channels::get_emergency_stop()) {
 #else
     if (!hal.util->get_soft_armed() || SRV_Channels::get_emergency_stop()) {
 #endif
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index 981c9b6d1f..28eb02a5a7 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -672,7 +672,9 @@ void Plane::set_servos(void)
 #if ADVANCED_FAILSAFE == ENABLED
     if (afs.should_crash_vehicle()) {
         afs.terminate_vehicle();
-        return;
+        if (!afs.terminating_vehicle_via_landing()) {
+            return;
+        }
     }
 #endif