From 3ab635f10077c3eb550316a6974a0cbb63427680 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir@azurdrones.com>
Date: Thu, 9 Aug 2018 19:08:55 +0200
Subject: [PATCH] AP_Gripper: use gripper_sim feedback

---
 libraries/AP_Gripper/AP_Gripper_EPM.cpp   |  3 ++
 libraries/AP_Gripper/AP_Gripper_Servo.cpp | 36 ++++++++++++++++++++++-
 libraries/AP_Gripper/AP_Gripper_Servo.h   |  4 +++
 3 files changed, 42 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Gripper/AP_Gripper_EPM.cpp b/libraries/AP_Gripper/AP_Gripper_EPM.cpp
index 8c86607990..65dafb272a 100644
--- a/libraries/AP_Gripper/AP_Gripper_EPM.cpp
+++ b/libraries/AP_Gripper/AP_Gripper_EPM.cpp
@@ -10,6 +10,7 @@
 #include "AP_Gripper_EPM.h"
 #include <AP_HAL/AP_HAL.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <GCS_MAVLink/GCS.h>
 #ifdef UAVCAN_NODE_FILE
 #include <fcntl.h>
 #include <stdio.h>
@@ -58,6 +59,7 @@ void AP_Gripper_EPM::grab()
         // move the servo output to the grab position
         SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
     }
+    gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
 }
 
 // release - move epm pwm output to the release position
@@ -80,6 +82,7 @@ void AP_Gripper_EPM::release()
         // move the servo to the release position
         SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm);
     }
+    gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing");
 }
 
 // neutral - return the epm pwm output to the neutral position
diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.cpp b/libraries/AP_Gripper/AP_Gripper_Servo.cpp
index a44d0772df..fdded637dc 100644
--- a/libraries/AP_Gripper/AP_Gripper_Servo.cpp
+++ b/libraries/AP_Gripper/AP_Gripper_Servo.cpp
@@ -1,4 +1,8 @@
 #include <AP_Gripper/AP_Gripper_Servo.h>
+#include <GCS_MAVLink/GCS.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+  #include <SITL/SITL.h>
+#endif
 
 extern const AP_HAL::HAL& hal;
 
@@ -13,6 +17,11 @@ void AP_Gripper_Servo::grab()
     // move the servo to the grab position
     SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
     action_timestamp = AP_HAL::millis();
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    is_releasing = false;
+    is_released = true;
+#endif
+    gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
 }
 
 void AP_Gripper_Servo::release()
@@ -20,6 +29,11 @@ void AP_Gripper_Servo::release()
     // move the servo to the release position
     SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm);
     action_timestamp = AP_HAL::millis();
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    is_releasing = true;
+    is_released = false;
+#endif
+    gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing");
 }
 
 bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const
@@ -35,11 +49,23 @@ bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const
         // (e.g. last action was a grab not a release)
         return false;
     }
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (is_releasing && AP::sitl()->gripper_sim.is_jaw_open()) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Gripper load released");
+        return true;
+    }
+    if (!is_releasing && !AP::sitl()->gripper_sim.is_jaw_open()) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbed");
+        return true;
+    }
+    return false;
+#else
     if (AP_HAL::millis() - action_timestamp < action_time) {
         // servo still moving....
         return false;
     }
     return true;
+#endif
 }
 
 
@@ -54,7 +80,15 @@ bool AP_Gripper_Servo::grabbed() const
 }
 
 // type-specific periodic updates:
-void AP_Gripper_Servo::update_gripper() { };
+void AP_Gripper_Servo::update_gripper() {
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (is_releasing && !is_released) {
+        is_released = released();
+    } else if (!is_releasing && is_released) {
+        is_released = !grabbed();
+    }
+#endif
+};
 
 bool AP_Gripper_Servo::valid() const
 {
diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.h b/libraries/AP_Gripper/AP_Gripper_Servo.h
index 071906a763..9b1161f70f 100644
--- a/libraries/AP_Gripper/AP_Gripper_Servo.h
+++ b/libraries/AP_Gripper/AP_Gripper_Servo.h
@@ -53,4 +53,8 @@ private:
     const uint16_t action_time = 3000; // ms; time to grab or release
 
     bool has_state_pwm(const uint16_t pwm) const;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    bool is_releasing;
+    bool is_released;
+#endif
 };