From 6e55c9554a08d24aa88a271220fc6a99191519d5 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Wed, 26 Jul 2017 22:23:40 -0700
Subject: [PATCH] Sub: Support common version of MAV_CMD_DO_FLIGHT_TERMINATION

---
 ArduSub/GCS_Mavlink.cpp | 18 +++++++++++-------
 ArduSub/GCS_Mavlink.h   |  1 +
 2 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index d054b92123..62c3320487 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -1027,13 +1027,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
             }
             break;
 
-        case MAV_CMD_DO_FLIGHTTERMINATION:
-            if (packet.param1 > 0.5f) {
-                sub.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)
@@ -1608,3 +1601,14 @@ AP_Rally *GCS_MAVLINK_Sub::get_rally() const
     return nullptr;
 #endif
 }
+
+MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) {
+    if (packet.param1 > 0.5f) {
+        sub.init_disarm_motors();
+        return MAV_RESULT_ACCEPTED;
+    }
+    return MAV_RESULT_FAILED;
+}
+
+// dummy method to avoid linking AFS
+bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate) { return false; }
diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index 25b3510d1c..2586dc03b7 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -20,6 +20,7 @@ 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;
 
     uint8_t sysid_my_gcs() const override;