From b0aeec4b6a38b254bac5975d2697a9d3b82eba88 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 8 Jun 2018 16:21:51 +1000
Subject: [PATCH] GCS_MAVLink: move handling of MAV_CMD_DO_GRIPPER up

---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 37 ++++++++++++++++++++++++++++
 2 files changed, 38 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index ba984fbe77..3ba7dea531 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -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);
 
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 0106bbbd50..fdea7e5c45 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -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;