From 4bf622dbaf76b1eccef906847047463dd92ff9c0 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Fri, 5 Aug 2022 19:04:03 +0930
Subject: [PATCH] AP_Gripper: Add feedback when already in state

---
 libraries/AP_Gripper/AP_Gripper_Servo.cpp | 26 +++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.cpp b/libraries/AP_Gripper/AP_Gripper_Servo.cpp
index 81d8e19dd0..04f94c63f5 100644
--- a/libraries/AP_Gripper/AP_Gripper_Servo.cpp
+++ b/libraries/AP_Gripper/AP_Gripper_Servo.cpp
@@ -16,6 +16,19 @@ void AP_Gripper_Servo::init_gripper()
 
 void AP_Gripper_Servo::grab()
 {
+    // check if we are already grabbing
+    if (config.state == AP_Gripper::STATE_GRABBING) {
+        // do nothing
+        return;
+    }
+
+    // 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");
+        return;
+    }
+
     // flag we are active and grabbing cargo
     config.state = AP_Gripper::STATE_GRABBING;
 
@@ -28,6 +41,19 @@ void AP_Gripper_Servo::grab()
 
 void AP_Gripper_Servo::release()
 {
+    // check if we are already releasing
+    if (config.state == AP_Gripper::STATE_RELEASING) {
+        // do nothing
+        return;
+    }
+
+    // check if we are already released
+    if (config.state == AP_Gripper::STATE_RELEASED) {
+        // inform user that we are already released
+        gcs().send_text(MAV_SEVERITY_INFO, "Gripper load released");
+        return;
+    }
+    
     // flag we are releasing cargo
     config.state = AP_Gripper::STATE_RELEASING;