From 9496ffea14fdf2e3f5b932517f9c0f2b1c4b0136 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 8 Feb 2019 10:45:23 +1100
Subject: [PATCH] Rover: enable gripper support

---
 APMrover2/APMrover2.cpp  | 3 +++
 APMrover2/Parameters.cpp | 6 ++++++
 APMrover2/Parameters.h   | 4 ++++
 APMrover2/Rover.h        | 1 +
 APMrover2/config.h       | 7 +++++++
 APMrover2/system.cpp     | 5 +++++
 6 files changed, 26 insertions(+)

diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index d4474f8abe..c6c2276cea 100644
--- a/APMrover2/APMrover2.cpp
+++ b/APMrover2/APMrover2.cpp
@@ -66,6 +66,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK_CLASS(RC_Channels,         (RC_Channels*)&rover.g2.rc_channels, read_aux_all,           10,    200),
     SCHED_TASK_CLASS(AP_BattMonitor,      &rover.battery,          read,           10,  300),
     SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events,  50,  200),
+#if GRIPPER_ENABLED == ENABLED
+    SCHED_TASK_CLASS(AP_Gripper,          &rover.g2.gripper,      update,         10,   75),
+#endif
     SCHED_TASK(rpm_update,             10,    100),
 #if MOUNT == ENABLED
     SCHED_TASK_CLASS(AP_Mount,            &rover.camera_mount,     update,         50,  200),
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 7c1d169e44..153354ca1c 100644
--- a/APMrover2/Parameters.cpp
+++ b/APMrover2/Parameters.cpp
@@ -688,6 +688,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("MIS_DONE_BEHAVE", 38, ParametersG2, mis_done_behave, 0),
 
+#if GRIPPER_ENABLED == ENABLED
+    // @Group: GRIP_
+    // @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
+    AP_SUBGROUPINFO(gripper, "GRIP_", 39, ParametersG2, AP_Gripper),
+#endif
+
     AP_GROUPEND
 };
 
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index a9e7279d60..5e39363c87 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -371,6 +371,10 @@ public:
     // Sprayer
     AC_Sprayer sprayer;
 
+#if GRIPPER_ENABLED
+    AP_Gripper gripper;
+#endif
+
     // Rally point library
     AP_Rally_Rover rally;
 
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 335ed029d2..2115448c84 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -59,6 +59,7 @@
 #include <AP_Scheduler/AP_Scheduler.h>              // main loop scheduler
 #include <AP_SerialManager/AP_SerialManager.h>      // Serial manager library
 #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
+#include <AP_Gripper/AP_Gripper.h>
 #include <AP_Stats/AP_Stats.h>                      // statistics library
 #include <AP_Terrain/AP_Terrain.h>
 #include <AP_Vehicle/AP_Vehicle.h>                  // needed for AHRS build
diff --git a/APMrover2/config.h b/APMrover2/config.h
index b00b861ef1..89b8e0c212 100644
--- a/APMrover2/config.h
+++ b/APMrover2/config.h
@@ -116,6 +116,13 @@
   #define CAMERA ENABLED
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// GRIPPER control
+//
+#ifndef GRIPPER_ENABLED
+ # define GRIPPER_ENABLED ENABLED
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // RALLY POINTS
 //
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index 08e38e5fac..a734d57a72 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -56,6 +56,11 @@ void Rover::init_ardupilot()
     BoardConfig_CAN.init();
 #endif
 
+    // init gripper
+#if GRIPPER_ENABLED == ENABLED
+    g2.gripper.init();
+#endif
+
     // initialise notify system
     notify.init();
     notify_mode(control_mode);