Rover: use handle_set_mode()

APMrover2/GCS_Mavlink.pde

@@ -918,45 +918,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_SET_MODE:
 		{
-            // decode
+            handle_set_mode(msg, mavlink_set_mode);
-            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;
-            }
-
             break;
         }
 

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. 
  */