AP_Gripper: add grabbed and released method

libraries/AP_Gripper/AP_Gripper.cpp

@@ -111,3 +111,21 @@ PASS_TO_BACKEND(release)
 PASS_TO_BACKEND(update)
 
 #undef PASS_TO_BACKEND
+
+
+#define PASS_TO_BACKEND(function_name)        \
+    bool AP_Gripper::function_name() const    \
+    {                                         \
+        if (!enabled()) {                     \
+            return false;                     \
+        }                                     \
+        if (backend != nullptr) {             \
+            return backend->function_name();  \
+        }                                     \
+        return false;                         \
+    }
+
+PASS_TO_BACKEND(released)
+PASS_TO_BACKEND(grabbed)
+
+#undef PASS_TO_BACKEND

libraries/AP_Gripper/AP_Gripper.h

@@ -35,6 +35,12 @@ public:
     // release - move the servo output to the release position
     void release();
 
+    // released - returns true if currently in released position
+    bool released() const;
+
+    // grabbed - returns true if currently in grabbed position
+    bool grabbed() const;
+
     // update - should be called at at least 10hz
     void update();
 

libraries/AP_Gripper/AP_Gripper_Backend.h

@@ -34,6 +34,12 @@ public:
     // release - move the servo output to the release position
     virtual void release() = 0;
 
+    // released - returns true if currently in released position
+    virtual bool released() const = 0;
+
+    // grabbed - returns true if currently in grabbed position
+    virtual bool grabbed() const = 0;
+
     // type-specific intiailisations:
     virtual void init_gripper() = 0;
 

libraries/AP_Gripper/AP_Gripper_EPM.cpp

@@ -54,7 +54,7 @@ void AP_Gripper_EPM::grab()
     else
 #endif
     {
-        // move the servo to the release position
+        // move the servo output to the grab position
         RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.grab_pwm);
     }
 }
@@ -121,3 +121,17 @@ UAVCANCommand AP_Gripper_EPM::make_uavcan_command(uint16_t command) const
     return cmd;
 }
 
+
+bool AP_Gripper_EPM::released() const
+{
+    // we assume instanteous releasing ATM:
+    return (config.state == AP_Gripper::STATE_GRABBED ||
+            config.state == AP_Gripper::STATE_GRABBING);
+}
+
+bool AP_Gripper_EPM::grabbed() const
+{
+    // we assume instanteous grabbing ATM:
+    return (config.state == AP_Gripper::STATE_GRABBED ||
+            config.state == AP_Gripper::STATE_GRABBING);
+}

libraries/AP_Gripper/AP_Gripper_EPM.h

@@ -35,6 +35,12 @@ public:
     // release - move the EPM pwm output to the release position
     void        release() override;
 
+    // grabbed - returns true if gripper in grabbed state
+    bool grabbed() const override;
+
+    // released - returns true if gripper in released state
+    bool released() const override;
+
     // update - moves the pwm back to neutral after the timeout has passed
     // should be called at at least 10hz
     void        update_gripper() override;

libraries/AP_Gripper/AP_Gripper_Servo.cpp

@@ -5,19 +5,52 @@ extern const AP_HAL::HAL& hal;
 void AP_Gripper_Servo::init_gripper()
 {
     // move the servo to the release position
-    RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.release_pwm);
+    release();
 }
 
 void AP_Gripper_Servo::grab()
 {
-    // move the servo to the release position
+    // move the servo to the grab position
     RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.grab_pwm);
+    action_timestamp = AP_HAL::millis();
 }
 
 void AP_Gripper_Servo::release()
 {
     // move the servo to the release position
     RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.release_pwm);
+    action_timestamp = AP_HAL::millis();
+}
+
+bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const
+{
+    // return true if servo is in position represented by pwm
+    int16_t current_pwm;
+    if (!RC_Channel_aux::get_radio(RC_Channel_aux::k_gripper, current_pwm)) {
+        // function not assigned to a channel, perhaps?
+        return false;
+    }
+    if (current_pwm != pwm) {
+        // last action did not set pwm to the current value
+        // (e.g. last action was a grabm not a release)
+        return false;
+    }
+    if (AP_HAL::millis() - action_timestamp < action_time) {
+        // servo still moving....
+        return false;
+    }
+    return true;
+}
+
+
+bool AP_Gripper_Servo::released() const
+{
+    return has_state_pwm(config.release_pwm);
+}
+
+bool AP_Gripper_Servo::grabbed() const
+{
+    return has_state_pwm(config.grab_pwm);
 }
 
 // type-specific periodic updates:

libraries/AP_Gripper/AP_Gripper_Servo.h

@@ -30,6 +30,12 @@ public:
     // release - move the servo output to the release position
     void release() override;
 
+    // grabbed - returns true if gripper in grabbed state
+    bool grabbed() const override;
+
+    // released - returns true if gripper in released state
+    bool released() const override;
+
 protected:
 
     // type-specific intiailisations:
@@ -37,4 +43,11 @@ protected:
 
     // type-specific periodic updates:
     void update_gripper() override;
+
+private:
+
+    uint32_t action_timestamp; // ms; time grab or release happened
+    const uint16_t action_time = 3000; // ms; time to grab or release
+
+    bool has_state_pwm(const uint16_t pwm) const;
 };