SITL: gripper closes faster, vary load carried by gripper

If gripper is closed on ground it is considered to be carrying
something

libraries/SITL/SIM_Gripper_Servo.cpp

@@ -39,13 +39,21 @@ void Gripper_Servo::update(const Aircraft::sitl_input &input)
     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);
     if (should_report()) {
-        ::fprintf(stderr, "position_demand=%f\n", position_demand);
+        ::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass);
-        printf("Position: %f mm\n", gap*position);
         last_report_us = now;
         reported_position = position;
     }
 
+    if (jaw_gap < 5) {
+        if (aircraft->on_ground()) {
+            load_mass = 1.0f; // attach the load
+        }
+    } else if (jaw_gap > 10) {
+        load_mass = 0.0f; // detach the load
+    }
+
     last_update_us = now;
     return;
 }

libraries/SITL/SIM_Gripper_Servo.h

@@ -47,7 +47,7 @@ private:
     const float gap = 30; // mm
 
     float position; // percentage
-    float position_slew_rate = 20; // percentage
+    float position_slew_rate = 35; // percentage
     float reported_position = -1; // unlikely
 
     uint64_t last_update_us;
@@ -57,7 +57,7 @@ private:
 
     // dangle load from a string:
     const float string_length = 2.0f; // metres
-    const float load_mass = 0.0f; // kilograms
+    float load_mass = 0.0f; // kilograms
 };
 
 }