Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: move handling of MAV_CMD_DO_GRIPPER up

Browse Source
This commit is contained in:
Peter Barker 2018-06-08 16:21:51 +10:00 committed by Francisco Ferreira
parent ef33d62cc3
commit b0aeec4b6a
2 changed files with 38 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -350,6 +350,7 @@ protected:
MAV_RESULT handle_command_long_message(mavlink_command_long_t &packet);
MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_send_banner(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_gripper(mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet);

37
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -20,6 +20,7 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_RangeFinder/RangeFinder_Backend.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Gripper/AP_Gripper.h>
#include "GCS.h"
@ -2580,6 +2581,38 @@ MAV_RESULT GCS_MAVLINK::handle_command_get_home_position(const mavlink_command_l
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(mavlink_command_long_t &packet)
{
AP_Gripper *gripper = AP::gripper();
if (gripper == nullptr) {
return MAV_RESULT_FAILED;
}
// param1 : gripper number (ignored)
// param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum.
if(!gripper->enabled()) {
return MAV_RESULT_FAILED;
}
MAV_RESULT result = MAV_RESULT_ACCEPTED;
switch ((uint8_t)packet.param2) {
case GRIPPER_ACTION_RELEASE:
gripper->release();
gcs().send_text(MAV_SEVERITY_INFO, "Gripper Released");
break;
case GRIPPER_ACTION_GRAB:
gripper->grab();
gcs().send_text(MAV_SEVERITY_INFO, "Gripper Grabbed");
break;
default:
result = MAV_RESULT_FAILED;
break;
}
return result;
}
MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &packet)
{
MAV_RESULT result = MAV_RESULT_FAILED;
@ -2611,6 +2644,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &pack
result = handle_command_camera(packet);
break;
case MAV_CMD_DO_GRIPPER:
result = handle_command_do_gripper(packet);
break;
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
result = handle_command_request_autopilot_capabilities(packet);
break;