From e4d3ed7e39ce4791e3142f4d31fc2694b234471e Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 8 Jun 2018 13:37:51 +1000
Subject: [PATCH] RC_Channel: move handling of sprayer and gripper in from
 Copter

---
 libraries/RC_Channel/RC_Channel.cpp | 51 ++++++++++++++++++++++++++++-
 libraries/RC_Channel/RC_Channel.h   |  4 ++-
 2 files changed, 53 insertions(+), 2 deletions(-)

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 3e5698b365..271e5e4da9 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -29,6 +29,9 @@ extern const AP_HAL::HAL& hal;
 
 #include <GCS_MAVLink/GCS.h>
 
+#include <AC_Sprayer/AC_Sprayer.h>
+#include <AP_Gripper/AP_Gripper.h>
+
 const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @Param: MIN
     // @DisplayName: RC min PWM
@@ -414,10 +417,14 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const aux_switch_
     case CAMERA_TRIGGER:
     case DO_NOTHING:
         break;
+    case GRIPPER:
+    case SPRAYER:
+        do_aux_function(ch_option, ch_flag);
+        break;
     default:
         gcs().send_text(MAV_SEVERITY_WARNING, "Failed to initialise RC function (%u)", ch_option);
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-        AP_HAL::panic("Initialisation failed");
+        AP_HAL::panic("RC function (%u) initialisation", ch_option);
 #endif
         break;
     }
@@ -473,6 +480,40 @@ void RC_Channel::do_aux_function_relay(const uint8_t relay, bool val)
     servorelayevents->do_set_relay(relay, val);
 }
 
+void RC_Channel::do_aux_function_sprayer(const aux_switch_pos_t ch_flag)
+{
+    AC_Sprayer *sprayer = AP::sprayer();
+    if (sprayer == nullptr) {
+        return;
+    }
+
+    sprayer->run(ch_flag == HIGH);
+    // if we are disarmed the pilot must want to test the pump
+    sprayer->test_pump((ch_flag == HIGH) && !hal.util->get_soft_armed());
+}
+
+void RC_Channel::do_aux_function_gripper(const aux_switch_pos_t ch_flag)
+{
+    AP_Gripper *gripper = AP::gripper();
+    if (gripper == nullptr) {
+        return;
+    }
+
+    switch(ch_flag) {
+    case LOW:
+        gripper->release();
+//        copter.Log_Write_Event(DATA_GRIPPER_RELEASE);
+        break;
+    case MIDDLE:
+        // nothing
+        break;
+    case HIGH:
+        gripper->grab();
+//        copter.Log_Write_Event(DATA_GRIPPER_GRAB);
+        break;
+    }
+}
+
 void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag)
 {
     switch(ch_option) {
@@ -480,6 +521,10 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
         do_aux_function_camera_trigger(ch_flag);
         break;
 
+    case GRIPPER:
+        do_aux_function_gripper(ch_flag);
+        break;
+
     case RELAY:
         do_aux_function_relay(0, ch_flag == HIGH);
         break;
@@ -493,6 +538,10 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
         do_aux_function_relay(3, ch_flag == HIGH);
         break;
 
+    case SPRAYER:
+        do_aux_function_sprayer(ch_flag);
+        break;
+
     default:
         gcs().send_text(MAV_SEVERITY_INFO, "Invalid channel option (%u)", ch_option);
         break;
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 10fb17dd7b..37f0943345 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -179,8 +179,10 @@ protected:
     virtual void init_aux_function(aux_func_t ch_option, aux_switch_pos_t);
     virtual void do_aux_function(aux_func_t ch_option, aux_switch_pos_t);
 
-    void do_aux_function_relay(uint8_t relay, bool val);
     void do_aux_function_camera_trigger(const aux_switch_pos_t ch_flag);
+    void do_aux_function_gripper(const aux_switch_pos_t ch_flag);
+    void do_aux_function_relay(uint8_t relay, bool val);
+    void do_aux_function_sprayer(const aux_switch_pos_t ch_flag);
 
     typedef int8_t modeswitch_pos_t;
     virtual void mode_switch_changed(modeswitch_pos_t new_pos) {