From 97c1785bef22c75846f0d849465579d3c9ebcafa Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Mon, 24 Jul 2017 23:34:09 -0700
Subject: [PATCH] AP_AdvancedFailsafe: Allow the GCS to request terminations

---
 .../AP_AdvancedFailsafe.cpp                   | 41 ++++++++++++++++---
 .../AP_AdvancedFailsafe/AP_AdvancedFailsafe.h |  3 ++
 2 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
index aec99ee944..cef4493253 100644
--- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
+++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
@@ -25,6 +25,8 @@
 #include <SRV_Channel/SRV_Channel.h>
 #include <GCS_MAVLink/GCS.h>
 
+#define TERMINATE 42
+
 extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
@@ -358,13 +360,40 @@ bool AP_AdvancedFailsafe::should_crash_vehicle(void)
         setup_IO_failsafe();
     }
 
-    // should we crash the plane? Only possible with
-    // FS_TERM_ACTTION set to 42
-    if (!_terminate || _terminate_action != 42) {
-        // not terminating
+    // determine if the vehicle should be crashed
+    // only possible if FS_TERM_ACTION is 42 and _terminate is non zero
+    // _terminate may be set via parameters, or a mavlink command
+    if (_terminate_action == TERMINATE && _terminate) {
+        // we are crashing
+        return true;
+    }
+
+    // continue flying
+    return false;
+}
+
+// update GCS based termination
+// returns true if AFS is in the desired termination state
+bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate) {
+    if (!_enable) {
+        gcs().send_text(MAV_SEVERITY_INFO, "AFS not enabled, can't terminate the vehicle");
         return false;
     }
 
-    // we are crashing
-    return true;
+    _terminate.set_and_notify(should_terminate ? 1 : 0);
+
+    // evaluate if we will crash or not, as AFS must be enabled, and TERM_ACTION has to be correct
+    bool is_terminating = should_crash_vehicle();
+
+    if(should_terminate == is_terminating) {
+        if (is_terminating) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Terminating due to GCS request");
+        } else {
+            gcs().send_text(MAV_SEVERITY_INFO, "Aborting termination due to GCS request");
+        }
+        return true;
+    } else if (should_terminate && _terminate_action != TERMINATE) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Unable to terminate, termination is not configured");
+    }
+    return false;
 }
diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h
index 9d0a56bad6..ee52a8d6b3 100644
--- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h
+++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h
@@ -72,6 +72,9 @@ public:
     // return true if we are terminating (deliberately crashing the vehicle)
     bool should_crash_vehicle(void);
 
+    // enables or disables a GCS based termination, returns true if AFS is in the desired termination state
+    bool gcs_terminate(bool should_terminate);
+
     // called to set all outputs to termination state
     virtual void terminate_vehicle(void) = 0;