From 5cfe4d5a8c76fbf8378196dc23b1c1689d94278e Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Mon, 24 Jul 2017 23:36:17 -0700
Subject: [PATCH] Copter: Expose the AFS instance to MAVLink

---
 ArduCopter/GCS_Mavlink.cpp | 35 ++++++++++++++++++++++++++++-------
 ArduCopter/GCS_Mavlink.h   |  2 ++
 2 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index 5f8067ed98..121f4f375b 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -969,13 +969,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
             }
             break;
 
-        case MAV_CMD_DO_FLIGHTTERMINATION:
-            if (packet.param1 > 0.5f) {
-                copter.init_disarm_motors();
-                result = MAV_RESULT_ACCEPTED;
-            }
-            break;
-
         case MAV_CMD_DO_SET_ROI:
             // param1 : regional of interest mode (not supported)
             // param2 : mission index/ target id (not supported)
@@ -1769,6 +1762,34 @@ AP_ServoRelayEvents *GCS_MAVLINK_Copter::get_servorelayevents() const
     return &copter.ServoRelayEvents;
 }
 
+AP_AdvancedFailsafe *GCS_MAVLINK_Copter::get_advanced_failsafe() const
+{
+#if ADVANCED_FAILSAFE == ENABLED
+    return &copter.g2.afs;
+#else
+    return nullptr;
+#endif
+}
+
+MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_long_t &packet) {
+    MAV_RESULT result = MAV_RESULT_FAILED;
+
+#if ADVANCED_FAILSAFE == ENABLED
+    if (GCS_MAVLINK::handle_flight_termination(packet) != MAV_RESULT_ACCEPTED) {
+#endif
+        if (packet.param1 > 0.5f) {
+            copter.init_disarm_motors();
+            result = MAV_RESULT_ACCEPTED;
+        }
+#if ADVANCED_FAILSAFE == ENABLED
+    } else {
+        result = MAV_RESULT_ACCEPTED;
+    }
+#endif
+
+    return result;
+}
+
 AP_Rally *GCS_MAVLINK_Copter::get_rally() const
 {
 #if AC_RALLY == ENABLED
diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h
index ae386b369f..46df9fe480 100644
--- a/ArduCopter/GCS_Mavlink.h
+++ b/ArduCopter/GCS_Mavlink.h
@@ -24,6 +24,8 @@ protected:
     AP_Camera *get_camera() const override;
     AP_ServoRelayEvents *get_servorelayevents() const override;
     AP_GPS *get_gps() const override;
+    MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override;
+    AP_AdvancedFailsafe *get_advanced_failsafe() const override;
 
     uint8_t sysid_my_gcs() const override;