AP_Mission: handle gripper mission items

libraries/AP_Mission/AP_Mission.cpp

@@ -261,12 +261,23 @@ void AP_Mission::update()
 
 bool AP_Mission::verify_command(const Mission_Command& cmd)
 {
-    return _cmd_verify_fn(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)
 {
-    return _cmd_start_fn(cmd);
+    switch (cmd.id) {
+    case MAV_CMD_DO_GRIPPER:
+        return start_command_do_gripper(cmd);
+    default:
+        return _cmd_start_fn(cmd);
+    }
 }
 
 ///

libraries/AP_Mission/AP_Mission.h

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

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -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;
+}