AP_Gripper: apply auto close to all backends.

ArduPilot · Aug 9, 2022 · 2d06261 · 2d06261
1 parent 4bf622d
commit 2d06261
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 18 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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;
 };