aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add support for DO_GRIPPER command

Browse Source
This commit is contained in:
Randy Mackay 2014-09-17 20:31:30 +09:00
parent 1ce8e453c2
commit bbe4438a6b
2 changed files with 52 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/GCS_Mavlink.pde
View File

@ -1226,6 +1226,28 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4);
break;
#if EPM_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER:
// param1 : gripper number (ignored)
// param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum.
if(!epm.enabled()) {
result = MAV_RESULT_FAILED;
} else {
result = MAV_RESULT_ACCEPTED;
switch ((uint8_t)packet.param2) {
case GRIPPER_ACTION_RELEASE:
epm.release();
break;
case GRIPPER_ACTION_GRAB:
epm.grab();
break;
default:
result = MAV_RESULT_FAILED;
break;
}
}
break;
#endif
default:
result = MAV_RESULT_UNSUPPORTED;
break;

30
ArduCopter/commands_logic.pde
View File

@ -21,6 +21,9 @@ static void do_roi(const AP_Mission::Mission_Command& cmd);
#if PARACHUTE == ENABLED
static void do_parachute(const AP_Mission::Mission_Command& cmd);
#endif
#if EPM_ENABLED == ENABLED
static void do_gripper(const AP_Mission::Mission_Command& cmd);
#endif
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
static bool verify_circle(const AP_Mission::Mission_Command& cmd);
static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
@ -167,6 +170,12 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
break;
#endif
#if EPM_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER: // Mission command to control EPM gripper
do_gripper(cmd);
break;
#endif
default:
// do nothing with unrecognized MAVLink messages
break;
@ -544,6 +553,27 @@ static void do_parachute(const AP_Mission::Mission_Command& cmd)
}
#endif
#if EPM_ENABLED == ENABLED
// do_gripper - control EPM gripper
static void do_gripper(const AP_Mission::Mission_Command& cmd)
{
// Note: we ignore the gripper num parameter because we only support one gripper
switch (cmd.content.gripper.action) {
case GRIPPER_ACTION_RELEASE:
epm.release();
Log_Write_Event(DATA_EPM_RELEASE);
break;
case GRIPPER_ACTION_GRAB:
epm.grab();
Log_Write_Event(DATA_EPM_GRAB);
break;
default:
// do nothing
break;
}
}
#endif
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/