Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Peter Barker 2021-02-18 12:01:41 +11:00 committed by Peter Barker
parent b2f47ba367
commit d4a0222f76
3 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Mission/AP_Mission.cpp
View File

@ -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:

10
libraries/AP_Mission/AP_Mission.h
View File

@ -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);

19
libraries/AP_Mission/AP_Mission_Commands.cpp
View File

@ -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();