SITL: Added damping factor to the tether dynamics

Author: snktshrma

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

libraries/SITL/SIM_Tether.h

@@ -55,6 +55,7 @@ class TetherSim {
     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 @@ class TetherSim {
     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
 };