From d4a0222f769b1d4047834747b2ce82eb747cab1e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Feb 2021 12:01:41 +1100 Subject: [PATCH] AP_Mission: allow an auxillary function to be triggered as a DO command --- libraries/AP_Mission/AP_Mission.cpp | 15 +++++++++++++++ libraries/AP_Mission/AP_Mission.h | 10 ++++++++++ libraries/AP_Mission/AP_Mission_Commands.cpp | 19 +++++++++++++++++++ 3 files changed, 44 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 8b087b2167..fffd819f10 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/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: diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index ab8877ce58..d9373386a7 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/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); diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index d1df7b54ad..c42397199a 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -7,6 +7,25 @@ #include #include +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();