GCS_MAVLink: add option to execute auxillary functions via mavlink

libraries/GCS_MAVLink/GCS.h

@@ -385,7 +385,7 @@ protected:
 
     virtual MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_set_home(const mavlink_command_long_t &packet);
-
+    MAV_RESULT handle_command_do_aux_function(const mavlink_command_long_t &packet);
     void handle_mission_request_list(const mavlink_message_t &msg);
     void handle_mission_request(const mavlink_message_t &msg) const;
     void handle_mission_request_int(const mavlink_message_t &msg) const;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -2466,6 +2466,17 @@ void GCS_MAVLINK::send_heartbeat() const
         system_status());
 }
 
+MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_long_t &packet)
+{
+    if (packet.param2 > 2) {
+        return MAV_RESULT_DENIED;
+    }
+    const RC_Channel::AUX_FUNC aux_func = (RC_Channel::AUX_FUNC)packet.param1;
+    const RC_Channel::AuxSwitchPos position = (RC_Channel::AuxSwitchPos)packet.param2;
+    rc().do_aux_function(aux_func, position);
+    return MAV_RESULT_ACCEPTED;
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_long_t &packet)
 {
     return set_message_interval((uint32_t)packet.param1, (int32_t)packet.param2);
@@ -4138,6 +4149,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
         }
         break;
 
+    case MAV_CMD_DO_AUX_FUNCTION:
+        result = handle_command_do_aux_function(packet);
+        break;
+
     case MAV_CMD_SET_MESSAGE_INTERVAL:
         result = handle_command_set_message_interval(packet);
         break;