AP_Mission: allow an auxillary function to be triggered as a DO command

libraries/AP_Mission/AP_Mission.cpp

@@ -300,6 +300,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
     case MAV_CMD_DO_PARACHUTE:
     case MAV_CMD_DO_SPRAYER:
+    case MAV_CMD_DO_AUX_FUNCTION:
     case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
         return true;
     default:
@@ -316,6 +317,8 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
 
     gcs().send_text(MAV_SEVERITY_INFO, "Mission: %u %s", cmd.index, cmd.type());
     switch (cmd.id) {
+    case MAV_CMD_DO_AUX_FUNCTION:
+        return start_command_do_aux_function(cmd);
     case MAV_CMD_DO_GRIPPER:
         return start_command_do_gripper(cmd);
     case MAV_CMD_DO_SET_SERVO:
@@ -1027,6 +1030,11 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
         cmd.p1 = packet.param1;                         // action 0=disable, 1=enable
         break;
 
+    case MAV_CMD_DO_AUX_FUNCTION:
+        cmd.content.auxfunction.function = packet.param1;
+        cmd.content.auxfunction.switchpos = packet.param2;
+        break;
+
     case MAV_CMD_DO_PARACHUTE:                         // MAV ID: 208
         cmd.p1 = packet.param1;                        // action 0=disable, 1=enable, 2=release.  See PARACHUTE_ACTION enum
         break;
@@ -1479,6 +1487,11 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
         packet.param1 = cmd.p1;                         // action 0=disable, 1=enable
         break;
 
+    case MAV_CMD_DO_AUX_FUNCTION:
+        packet.param1 = cmd.content.auxfunction.function;
+        packet.param2 = cmd.content.auxfunction.switchpos;
+        break;
+
     case MAV_CMD_DO_INVERTED_FLIGHT:                    // MAV ID: 210
         packet.param1 = cmd.p1;                         // normal=0 inverted=1
         break;
@@ -2247,6 +2260,8 @@ const char *AP_Mission::Mission_Command::type() const
         return "Parachute";
     case MAV_CMD_DO_SPRAYER:
         return "Sprayer";
+    case MAV_CMD_DO_AUX_FUNCTION:
+        return "AuxFunction";
     case MAV_CMD_DO_MOUNT_CONTROL:
         return "MountControl";
     case MAV_CMD_DO_WINCH:

libraries/AP_Mission/AP_Mission.h

@@ -148,6 +148,12 @@ public:
         uint8_t action;         // action (0 = release, 1 = grab)
     };
 
+    // AUX_FUNCTION command structure
+    struct PACKED AuxFunction {
+        uint16_t function;  // from RC_Channel::AUX_FUNC
+        uint8_t switchpos;  // from RC_Channel::AuxSwitchPos
+    };
+
     // high altitude balloon altitude wait
     struct PACKED Altitude_Wait {
         float altitude; // meters
@@ -241,6 +247,9 @@ public:
         // do-gripper
         Gripper_Command gripper;
 
+        // arbitrary aux function
+        AuxFunction auxfunction;
+
         // do-guided-limits
         Guided_Limits_Command guided_limits;
 
@@ -691,6 +700,7 @@ private:
     static HAL_Semaphore _rsem;
 
     // mission items common to all vehicles:
+    bool start_command_do_aux_function(const AP_Mission::Mission_Command& cmd);
     bool start_command_do_gripper(const AP_Mission::Mission_Command& cmd);
     bool start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd);
     bool start_command_camera(const AP_Mission::Mission_Command& cmd);

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -7,6 +7,25 @@
 #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
 #include <AC_Sprayer/AC_Sprayer.h>
 
+bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd)
+{
+    const RC_Channel::AUX_FUNC function = (RC_Channel::AUX_FUNC)cmd.content.auxfunction.function;
+    const RC_Channel::AuxSwitchPos pos = (RC_Channel::AuxSwitchPos)cmd.content.auxfunction.switchpos;
+
+    // sanity check the switch position.  Could map from the mavlink
+    // enumeration if we were really keen
+    switch (pos) {
+    case RC_Channel::AuxSwitchPos::HIGH:
+    case RC_Channel::AuxSwitchPos::MIDDLE:
+    case RC_Channel::AuxSwitchPos::LOW:
+        break;
+    default:
+        return false;
+    }
+    rc().do_aux_function(function, pos);
+    return true;
+}
+
 bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd)
 {
     AP_Gripper *gripper = AP::gripper();