From ed38b9439f153920bb4d858c4006720dc7345f63 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 6 Aug 2022 14:35:17 +0100
Subject: [PATCH] AP_Gripper: apply auto close to all backends.

---
 libraries/AP_Gripper/AP_Gripper_Backend.cpp |  8 ++++++++
 libraries/AP_Gripper/AP_Gripper_Backend.h   |  2 ++
 libraries/AP_Gripper/AP_Gripper_EPM.h       |  3 ---
 libraries/AP_Gripper/AP_Gripper_Servo.cpp   | 20 +++++++-------------
 libraries/AP_Gripper/AP_Gripper_Servo.h     |  2 --
 5 files changed, 17 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_Gripper/AP_Gripper_Backend.cpp b/libraries/AP_Gripper/AP_Gripper_Backend.cpp
index 5e82144491cc5..3c4680651a57e 100644
--- a/libraries/AP_Gripper/AP_Gripper_Backend.cpp
+++ b/libraries/AP_Gripper/AP_Gripper_Backend.cpp
@@ -1,4 +1,5 @@
 #include "AP_Gripper_Backend.h"
+#include <AP_Math/AP_Math.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -11,4 +12,11 @@ void AP_Gripper_Backend::init()
 void AP_Gripper_Backend::update()
 {
     update_gripper();
+
+    // close the gripper again if autoclose_time > 0.0
+    if (config.state == AP_Gripper::STATE_RELEASED && (_last_grab_or_release > 0) &&
+        (is_positive(config.autoclose_time)) &&
+        (AP_HAL::millis() - _last_grab_or_release > (config.autoclose_time * 1000.0))) {
+        grab();
+    }
 }
diff --git a/libraries/AP_Gripper/AP_Gripper_Backend.h b/libraries/AP_Gripper/AP_Gripper_Backend.h
index 30fb9f8a1dfc5..4557ee8988835 100644
--- a/libraries/AP_Gripper/AP_Gripper_Backend.h
+++ b/libraries/AP_Gripper/AP_Gripper_Backend.h
@@ -51,5 +51,7 @@ class AP_Gripper_Backend {
 
 protected:
 
+    uint32_t _last_grab_or_release; // ms; time last grab or release happened
+
     struct AP_Gripper::Backend_Config &config;
 };
diff --git a/libraries/AP_Gripper/AP_Gripper_EPM.h b/libraries/AP_Gripper/AP_Gripper_EPM.h
index 6b3b9d18bac1b..587c0116200c9 100644
--- a/libraries/AP_Gripper/AP_Gripper_EPM.h
+++ b/libraries/AP_Gripper/AP_Gripper_EPM.h
@@ -56,7 +56,4 @@ class AP_Gripper_EPM : public AP_Gripper_Backend {
 
     // UAVCAN driver fd
     int _uavcan_fd = -1;
-
-    // internal variables
-    uint32_t    _last_grab_or_release;
 };
diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.cpp b/libraries/AP_Gripper/AP_Gripper_Servo.cpp
index 04f94c63f5acd..6c6f43eb1c576 100644
--- a/libraries/AP_Gripper/AP_Gripper_Servo.cpp
+++ b/libraries/AP_Gripper/AP_Gripper_Servo.cpp
@@ -25,7 +25,7 @@ void AP_Gripper_Servo::grab()
     // check if we are already grabbed
     if (config.state == AP_Gripper::STATE_GRABBED) {
         // inform user that we are already grabbed
-        gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
+        gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbed");
         return;
     }
 
@@ -34,7 +34,7 @@ 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();
+    _last_grab_or_release = AP_HAL::millis();
     gcs().send_text(MAV_SEVERITY_INFO, "Gripper load grabbing");
     AP::logger().Write_Event(LogEvent::GRIPPER_GRAB);
 }
@@ -59,7 +59,7 @@ 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();
+    _last_grab_or_release = AP_HAL::millis();
     gcs().send_text(MAV_SEVERITY_INFO, "Gripper load releasing");
     AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE);
 }
@@ -77,7 +77,7 @@ 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 (AP_HAL::millis() - action_timestamp < SERVO_ACTUATION_TIME) {
+    if (AP_HAL::millis() - _last_grab_or_release < SERVO_ACTUATION_TIME) {
         // servo still moving....
         return false;
     }
@@ -95,21 +95,15 @@ bool AP_Gripper_Servo::grabbed() const
 }
 
 // type-specific periodic updates:
-void AP_Gripper_Servo::update_gripper() {
+void AP_Gripper_Servo::update_gripper()
+{
     // Check for successful grabbed or released
     if (config.state == AP_Gripper::STATE_GRABBING && has_state_pwm(config.grab_pwm)) {
         config.state = AP_Gripper::STATE_GRABBED;
     } else if (config.state == AP_Gripper::STATE_RELEASING && has_state_pwm(config.release_pwm)) {
         config.state = AP_Gripper::STATE_RELEASED;
     }
-
-    // close the gripper again if autoclose_time > 0.0
-    if (config.state == AP_Gripper::STATE_RELEASED &&
-        (is_positive(config.autoclose_time)) &&
-        (AP_HAL::millis() - action_timestamp > (config.autoclose_time * 1000.0))) {
-        grab();
-    }
-};
+}
 
 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 bdbc381897b8a..a98b0ca03c548 100644
--- a/libraries/AP_Gripper/AP_Gripper_Servo.h
+++ b/libraries/AP_Gripper/AP_Gripper_Servo.h
@@ -51,7 +51,5 @@ class AP_Gripper_Servo : public AP_Gripper_Backend {
 
 private:
 
-    uint32_t action_timestamp; // ms; time grab or release happened
-
     bool has_state_pwm(const uint16_t pwm) const;
 };