diff --git a/libraries/SITL/SIM_Gripper_Servo.cpp b/libraries/SITL/SIM_Gripper_Servo.cpp index b2e4d27ef3..d1c93c1631 100644 --- a/libraries/SITL/SIM_Gripper_Servo.cpp +++ b/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); - printf("Position: %f mm\n", gap*position); + ::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass); 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; } diff --git a/libraries/SITL/SIM_Gripper_Servo.h b/libraries/SITL/SIM_Gripper_Servo.h index dd640be2af..c255baf879 100644 --- a/libraries/SITL/SIM_Gripper_Servo.h +++ b/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 }; }