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

---
 APMrover2/GCS_Mavlink.pde | 42 ++-------------------------------------
 APMrover2/system.pde      | 18 +++++++++++++++++
 2 files changed, 20 insertions(+), 40 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index ef4d717905..5bf23fd89b 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -918,47 +918,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_SET_MODE:
 		{
-            // 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;
-            }
-
-            // 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();
-                } else if (packet.custom_mode == 1) {
-                    // turn safety on (no pwm outputs to the motors)
-                    hal.rcout->force_safety_on();
-                }
-                break;
-            }
-
-            // check if we are setting the control mode
-            if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
-                // we ignore base_mode as there is no sane way to map
-                // from that bitmap to a APM flight mode. We rely on
-                // custom_mode instead.
-                break;
-            }
-            switch (packet.custom_mode) {
-            case MANUAL:
-            case HOLD:
-            case LEARNING:
-            case STEERING:
-            case AUTO:
-            case RTL:
-                set_mode((enum mode)packet.custom_mode);
-                break;
-            }
-
+            handle_set_mode(msg, mavlink_set_mode);
             break;
-		}
+        }
 
     case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
         {
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index f692508091..ae6992823d 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -325,6 +325,24 @@ static void set_mode(enum mode mode)
     }
 }
 
+/*
+  set_mode() wrapper for MAVLink SET_MODE
+ */
+static bool mavlink_set_mode(uint8_t mode)
+{
+    switch (mode) {
+    case MANUAL:
+    case HOLD:
+    case LEARNING:
+    case STEERING:
+    case AUTO:
+    case RTL:
+        set_mode((enum mode)mode);
+        return true;
+    }
+    return false;
+}
+
 /*
   called to set/unset a failsafe event. 
  */