Plane: Add support for AP_Gripper

ArduPlane/ArduPlane.cpp

@@ -84,6 +84,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(avoidance_adsb_update,  10,    100),
     SCHED_TASK(button_update,           5,    100),
     SCHED_TASK(stats_update,            1,    100),
+#if GRIPPER_ENABLED == ENABLED
+    SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75),
+#endif
 };
 
 /*

ArduPlane/GCS_Mavlink.cpp

@@ -1422,6 +1422,31 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             }
             break;
             
+#if GRIPPER_ENABLED == ENABLED
+        case MAV_CMD_DO_GRIPPER:
+            // param1 : gripper number (ignored)
+            // param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum.
+            if(!plane.g2.gripper.enabled()) {
+                result = MAV_RESULT_FAILED;
+            } else {
+                result = MAV_RESULT_ACCEPTED;
+                switch ((uint8_t)packet.param2) {
+                    case GRIPPER_ACTION_RELEASE:
+                        plane.g2.gripper.release();
+                        gcs().send_text(MAV_SEVERITY_INFO, "Gripper Released");
+                        break;
+                    case GRIPPER_ACTION_GRAB:
+                        plane.g2.gripper.grab();
+                        gcs().send_text(MAV_SEVERITY_INFO, "Gripper Grabbed");
+                        break;
+                    default:
+                        result = MAV_RESULT_FAILED;
+                        break;
+                }
+            }
+            break;
+#endif
+
         default:
             result = handle_command_long_message(packet);
             break;

ArduPlane/Parameters.cpp

@@ -1186,6 +1186,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("HOME_RESET_ALT", 11, ParametersG2, home_reset_threshold, 0),
 
+#if GRIPPER_ENABLED == ENABLED
+    // @Group: GRIP_
+    // @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
+    AP_SUBGROUPINFO(gripper, "GRIP_", 12, ParametersG2, AP_Gripper),
+#endif
+
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -545,6 +545,11 @@ public:
     // home reset altitude threshold
     AP_Int8 home_reset_threshold;
 
+#if GRIPPER_ENABLED == ENABLED
+    // Payload Gripper
+    AP_Gripper gripper;
+#endif
+
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -95,6 +95,7 @@
 #include <AP_ADSB/AP_ADSB.h>
 #include <AP_Button/AP_Button.h>
 #include <AP_ICEngine/AP_ICEngine.h>
+#include <AP_Gripper/AP_Gripper.h>
 #include <AP_Landing/AP_Landing.h>
 
 #include "GCS_Mavlink.h"

ArduPlane/commands_logic.cpp

@@ -227,6 +227,23 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
                                             cmd.content.do_engine_control.cold_start,
                                             cmd.content.do_engine_control.height_delay_cm*0.01f);
         break;
+#if GRIPPER_ENABLED == ENABLED
+    case MAV_CMD_DO_GRIPPER:                            // Mission command to control gripper
+        switch (cmd.content.gripper.action) {
+            case GRIPPER_ACTION_RELEASE:
+                plane.g2.gripper.release();
+                gcs().send_text(MAV_SEVERITY_INFO, "Gripper Released");
+                break;
+            case GRIPPER_ACTION_GRAB:
+                plane.g2.gripper.grab();
+                gcs().send_text(MAV_SEVERITY_INFO, "Gripper Grabbed");
+                break;
+            default:
+                // do nothing
+                break;
+        }
+        break;
+#endif
     }
 
     return true;
@@ -328,6 +345,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_DO_MOUNT_CONTROL:
     case MAV_CMD_DO_VTOL_TRANSITION:
     case MAV_CMD_DO_ENGINE_CONTROL:
+    case MAV_CMD_DO_GRIPPER:
         return true;
 
     default:

ArduPlane/config.h

@@ -340,9 +340,18 @@
 #define PARACHUTE ENABLED
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Payload Gripper
+#ifndef GRIPPER_ENABLED
+ #if HAL_MINIMIZE_FEATURES
+  # define GRIPPER_ENABLED DISABLED
+ #else 
+  # define GRIPPER_ENABLED ENABLED
+ #endif
+#endif
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 # define HAVE_PX4_MIXER 1
 #else
 # define HAVE_PX4_MIXER 0
 #endif
-

ArduPlane/make.inc

@@ -60,3 +60,4 @@ LIBRARIES += AP_Landing
 LIBRARIES += AP_Beacon
 LIBRARIES += PID
 LIBRARIES += AP_Soaring
+LIBRARIES += AP_Gripper

ArduPlane/system.cpp

@@ -201,6 +201,11 @@ void Plane::init_ardupilot()
     }
 #endif
 
+// init cargo gripper
+#if GRIPPER_ENABLED == ENABLED
+    g2.gripper.init();
+#endif
+
     // disable safety if requested
     BoardConfig.init_safety();
 }

ArduPlane/wscript

@@ -32,7 +32,8 @@ def build(bld):
             'AP_Landing',
             'AP_Beacon',
             'PID',
-            'AP_Soaring'
+            'AP_Soaring',
+            'AP_Gripper'
         ],
     )