From f2e6fa3fb000ab739e26871dad95b8febbbf798d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 1 Oct 2014 14:19:31 +1000
Subject: [PATCH] Copter: use handle_set_mode()

---
 ArduCopter/GCS_Mavlink.pde | 33 +--------------------------------
 1 file changed, 1 insertion(+), 32 deletions(-)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 005b584491..bcda5e9695 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -887,38 +887,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_SET_MODE:       // MAV ID: 11
     {
-        // decode
-        mavlink_set_mode_t packet;
-        mavlink_msg_set_mode_decode(msg, &packet);
-
-        // exit immediately if this command is not meant for this vehicle
-        if (mavlink_check_target(packet.target_system, 0)) {
-            break;
-        }
-
-        // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
-        if (packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-            if (set_mode(packet.custom_mode)) {
-                result = MAV_RESULT_ACCEPTED;
-            }
-        }
-
-        // set the safety switch position
-        if (packet.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
-            if (packet.custom_mode == 0) {
-                // turn safety off (pwm outputs flow to the motors)
-                hal.rcout->force_safety_off();
-                result = MAV_RESULT_ACCEPTED;
-            } else if (packet.custom_mode == 1) {
-                // turn safety on (no pwm outputs to the motors)
-                if (hal.rcout->force_safety_on()) {
-                    result = MAV_RESULT_ACCEPTED;
-                }
-            }
-        }
-
-        // send ACK or NAK
-        mavlink_msg_command_ack_send_buf(msg, chan, MAVLINK_MSG_ID_SET_MODE, result);
+        handle_set_mode(msg, set_mode);
         break;
     }