Copter: use handle_set_mode()

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;
     }