From ef61ebf09bdc0d33fa54a73e56adaa9e27943ab6 Mon Sep 17 00:00:00 2001 From: snktshrma Date: Fri, 14 Mar 2025 20:48:22 +0530 Subject: [PATCH] SITL: Added damping factor to the tether dynamics --- libraries/SITL/SIM_Tether.cpp | 36 ++++++++++++++++++++++++----------- libraries/SITL/SIM_Tether.h | 5 ++++- 2 files changed, 29 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/SIM_Tether.cpp b/libraries/SITL/SIM_Tether.cpp index 005347b4c25..11da8f57319 100644 --- a/libraries/SITL/SIM_Tether.cpp +++ b/libraries/SITL/SIM_Tether.cpp @@ -75,6 +75,13 @@ const AP_Param::GroupInfo TetherSim::var_info[] = { // @User: Advanced AP_GROUPINFO("SPGCNST", 6, TetherSim, tether_spring_constant, 100), + // @Param: DMPCNST + // @DisplayName: Tether Damping Constant + // @Description: Damping constant for the tether to simulate resistance based on change in stretch + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("DMPCNST", 7, TetherSim, tether_damping_constant, 10), + AP_GROUPEND }; @@ -266,33 +273,40 @@ void TetherSim::update_tether_force(const Location& veh_pos, float dt) // Calculate the stretch beyond the maximum length float stretch = MAX(tether_length - max_line_length, 0.0f); - // Apply a spring-like penalty force proportional to the stretch - float penalty_force_magnitude = tether_spring_constant * stretch; + // Calculate the stretch rate beyond the maximum length + float stretch_rate = (stretch - prev_stretch)/dt; + + // Apply a spring and damping penalty force proportional to the stretch + float penalty_force_magnitude = tether_spring_constant * stretch + tether_damping_constant * stretch_rate; // Direction of force is along the tether, pulling toward the anchor - veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude; + veh_forces_teth = tether_vector.normalized() * penalty_force_magnitude; GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Exceeded maximum length."); - return; - } + prev_stretch = stretch; - if (tether_stuck) { + } else if (tether_stuck) { // Calculate the stretch beyond the maximum length float stretch = MAX(tether_length - tether_not_stuck_length, 0.0f); + // Calculate the stretch rate beyond the maximum length + float stretch_rate = (stretch - prev_stretch)/dt; + // Apply a spring-like penalty force proportional to the stretch - float penalty_force_magnitude = tether_spring_constant * stretch; + float penalty_force_magnitude = tether_spring_constant * stretch + tether_damping_constant * stretch_rate; // Direction of force is directly along the tether, towards the tether anchor point - veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude; + veh_forces_teth = tether_vector.normalized() * penalty_force_magnitude; GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Stuck."); - return; + prev_stretch = stretch; + } else { tether_not_stuck_length = tether_length; + veh_forces_teth.zero(); } // Step 3: Calculate the weight of the tether being lifted @@ -302,8 +316,8 @@ void TetherSim::update_tether_force(const Location& veh_pos, float dt) // Step 4: Calculate the tension force Vector3f tension_force_NED = tether_vector.normalized() * tether_weight_force; - // Step 5: Apply the force to the vehicle - veh_forces_ef = tension_force_NED; + // Step 5: Apply the total force to the vehicle + veh_forces_ef = veh_forces_teth + tension_force_NED; } #endif diff --git a/libraries/SITL/SIM_Tether.h b/libraries/SITL/SIM_Tether.h index 4a7366e1089..1f4a948d55b 100644 --- a/libraries/SITL/SIM_Tether.h +++ b/libraries/SITL/SIM_Tether.h @@ -55,6 +55,7 @@ private: AP_Int8 sys_id; // MAVLink system ID for GCS reporting AP_Int8 tether_stuck; // Set to 1 to simulate a stuck tether AP_Float tether_spring_constant; // Spring constant for modeling tether stretch + AP_Float tether_damping_constant; // Damping constant // Send MAVLink messages to the GCS void send_report(); @@ -85,7 +86,9 @@ private: mavlink_status_t mav_status; // Status of MAVLink communication // Tether simulation variables - Vector3f veh_forces_ef; // Earth-frame forces on the vehicle due to the tether + float prev_stretch = 0.0f; // store stretch beyond maximum in last calculation + Vector3f veh_forces_ef; // Effective Earth-frame forces on the vehicle due to the tether + Vector3f veh_forces_teth; // Earth-frame forces on the vehicle due to the spring and damping forces on tether float tether_length = 0.0f; // Current tether length in meters float tether_not_stuck_length = 0.0f; // Tether length when the tether is not stuck };