SITL: Better gripper sim options

libraries/SITL/SIM_Gripper_Servo.cpp

@@ -40,6 +40,29 @@ const AP_Param::GroupInfo Gripper_Servo::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("PIN", 1, Gripper_Servo, gripper_servo_pin, -1),
 
+    // @Param: GRAB
+    // @DisplayName: Gripper Grab PWM
+    // @Description: PWM value in microseconds sent to Gripper to initiate grabbing the cargo
+    // @User: Advanced
+    // @Range: 1000 2000
+    // @Units: PWM
+    AP_GROUPINFO("GRAB", 2, Gripper_Servo, grab_pwm, SIM_GRIPPER_GRAB_PWM_DEFAULT),
+
+    // @Param: RELEASE
+    // @DisplayName: Gripper Release PWM
+    // @Description: PWM value in microseconds sent to Gripper to release the cargo
+    // @User: Advanced
+    // @Range: 1000 2000
+    // @Units: PWM
+    AP_GROUPINFO("RELEASE", 3, Gripper_Servo, release_pwm, SIM_GRIPPER_RELEASE_PWM_DEFAULT),
+
+    // @Param: REVERSE
+    // @DisplayName: Gripper close direction
+    // @Description: Reverse the closing direction.
+    // @User: Advanced
+    // @Values: 0:Normal,1:Reverse
+    AP_GROUPINFO("REVERSE", 4, Gripper_Servo, reverse, 0),
+
     AP_GROUPEND
 };
 
@@ -58,16 +81,20 @@ void Gripper_Servo::update(const struct sitl_input &input)
         last_update_us = now;
         return;
     }
-
-    float position_demand = (gripper_pwm - 1000) * 0.001f;
-    if (is_negative(position_demand)) { // never updated
-        position_demand = 0.0f;
+    const int16_t diff_pwm = abs(grab_pwm - release_pwm);
+    float position_demand = (gripper_pwm - diff_pwm) * 0.001f;
+    if (gripper_pwm < MIN(grab_pwm, release_pwm) || position_demand > 1.0f) { // never updated
+        position_demand = position;
     }
 
     const float position_max_change = position_slew_rate / 100.0f * dt;
     position = constrain_float(position_demand, position - position_max_change, position + position_max_change);
-
-    const float jaw_gap = gap * (1.0f - position);
+    float jaw_gap;
+    if ((release_pwm < grab_pwm && reverse) || (release_pwm > grab_pwm && !reverse)) {
+        jaw_gap = gap * position;
+    } else {
+        jaw_gap = gap * (1.0f - position);
+    }
     if (should_report()) {
         ::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass);
         last_report_us = now;
@@ -77,9 +104,11 @@ void Gripper_Servo::update(const struct sitl_input &input)
     if (jaw_gap < 5) {
         if (altitude <= 0.0f) {
             load_mass = 1.0f; // attach the load
+            jaw_open = false;
         }
     } else if (jaw_gap > 10) {
         load_mass = 0.0f; // detach the load
+        jaw_open = true;
     }
 
     last_update_us = now;

libraries/SITL/SIM_Gripper_Servo.h

@@ -37,13 +37,19 @@ public:
     void set_alt(float alt) {altitude = alt;};
     static const struct AP_Param::GroupInfo var_info[];
     bool is_enabled() const {return static_cast<bool>(gripper_enable);}
+    bool is_jaw_open() const {return jaw_open;}
 
 private:
+    static constexpr int16_t SIM_GRIPPER_GRAB_PWM_DEFAULT = 2000;
+    static constexpr int16_t SIM_GRIPPER_RELEASE_PWM_DEFAULT = 1000;
     AP_Int8  gripper_enable;  // enable gripper sim
     AP_Int8  gripper_servo_pin;
+    AP_Int16 grab_pwm;              // PWM value sent to Gripper to initiate grabbing the cargo
+    AP_Int16 release_pwm;           // PWM value sent to Gripper to release the cargo
+    AP_Int8 reverse;                // reverse closing direction
     const uint32_t report_interval = 1000000; // microseconds
     uint64_t last_report_us;
-
+    bool jaw_open = false;
     const float gap = 30; // mm
     float altitude;
     float position; // percentage