From 1650979a2b17d7ad05a19d297ff7eef1b9f8fb79 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 8 Feb 2019 19:41:28 +1100
Subject: [PATCH] GCS_MAVLink: add support for MAV_CMD_DEBUG_TRAP

---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 16 ++++++++++++++++
 2 files changed, 17 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 3d9568c72a..d88765421e 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -440,6 +440,7 @@ protected:
     MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet);
+    MAV_RESULT handle_command_debug_trap(const mavlink_command_long_t &packet);
 
     void handle_optical_flow(const mavlink_message_t &msg);
 
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 7c6cf09d78..cca48de242 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3608,6 +3608,18 @@ MAV_RESULT GCS_MAVLINK::handle_command_get_home_position(const mavlink_command_l
     return MAV_RESULT_ACCEPTED;
 }
 
+MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_long_t &packet)
+{
+    // magic number must be supplied to trap; you must *really* mean it.
+    if (uint32_t(packet.param1) != 32451) {
+        return MAV_RESULT_DENIED;
+    }
+    if (hal.util->trap()) {
+        return MAV_RESULT_ACCEPTED;
+    }
+    return MAV_RESULT_UNSUPPORTED;
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_long_t &packet)
 {
     AP_Gripper *gripper = AP::gripper();
@@ -3775,6 +3787,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
         result = handle_command_get_home_position(packet);
         break;
 
+    case MAV_CMD_DEBUG_TRAP:
+        result = handle_command_debug_trap(packet);
+        break;
+
     case MAV_CMD_PREFLIGHT_STORAGE:
         if (is_equal(packet.param1, 2.0f)) {
             AP_Param::erase_all();