From cc1f27ad952129f2e9ad922a2c5a26e47e112a1b Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Wed, 6 Jan 2016 14:05:22 -0800
Subject: [PATCH] Copter: add DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
 option

---
 ArduCopter/GCS_Mavlink.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index 9b26468498..2c66509087 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -1008,7 +1008,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_SET_MODE:       // MAV ID: 11
     {
+#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
+        if (!copter.failsafe.radio) {
+            handle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::set_mode, bool, uint8_t));
+        } else {
+            // don't allow mode changes while in radio failsafe
+            mavlink_msg_command_ack_send_buf(msg, chan, MAVLINK_MSG_ID_SET_MODE, MAV_RESULT_FAILED);
+        }
+#else
         handle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::set_mode, bool, uint8_t));
+#endif
         break;
     }