mirror of https://github.com/ArduPilot/ardupilot
AP_Mission: handle gripper mission items
This commit is contained in:
parent
8ebec6a237
commit
182d3634aa
|
@ -261,12 +261,23 @@ void AP_Mission::update()
|
|||
|
||||
bool AP_Mission::verify_command(const Mission_Command& cmd)
|
||||
{
|
||||
switch (cmd.id) {
|
||||
// do-commands always return true for verify:
|
||||
case MAV_CMD_DO_GRIPPER:
|
||||
return true;
|
||||
default:
|
||||
return _cmd_verify_fn(cmd);
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_Mission::start_command(const Mission_Command& cmd)
|
||||
{
|
||||
switch (cmd.id) {
|
||||
case MAV_CMD_DO_GRIPPER:
|
||||
return start_command_do_gripper(cmd);
|
||||
default:
|
||||
return _cmd_start_fn(cmd);
|
||||
}
|
||||
}
|
||||
|
||||
///
|
||||
|
|
|
@ -578,6 +578,9 @@ private:
|
|||
// multi-thread support. This is static so it can be used from
|
||||
// const functions
|
||||
static HAL_Semaphore_Recursive _rsem;
|
||||
|
||||
// mission items common to all vehicles:
|
||||
bool start_command_do_gripper(const AP_Mission::Mission_Command& cmd);
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
|
|
|
@ -0,0 +1,32 @@
|
|||
#include "AP_Mission.h"
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_Gripper/AP_Gripper.h>
|
||||
|
||||
bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
AP_Gripper *gripper = AP::gripper();
|
||||
if (gripper == nullptr) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Note: we ignore the gripper num parameter because we only
|
||||
// support one gripper
|
||||
switch (cmd.content.gripper.action) {
|
||||
case GRIPPER_ACTION_RELEASE:
|
||||
gripper->release();
|
||||
// Log_Write_Event(DATA_GRIPPER_RELEASE);
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Gripper Released");
|
||||
break;
|
||||
case GRIPPER_ACTION_GRAB:
|
||||
gripper->grab();
|
||||
// Log_Write_Event(DATA_GRIPPER_GRAB);
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Gripper Grabbed");
|
||||
break;
|
||||
default:
|
||||
// do nothing
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
Loading…
Reference in New Issue