SITL: Add simulator for tethered vehicle

libraries/SITL/SIM_Aircraft.cpp

@@ -839,6 +839,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
     sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth, wind_ef);
 #endif
 
+    // update tether
+#if AP_SIM_TETHER_ENABLED
+    sitl->models.tether_sim.update(location);
+#endif
+
     // allow for changes in physics step
     adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
 }
@@ -1273,18 +1278,26 @@ void Aircraft::add_twist_forces(Vector3f &rot_accel)
     }
 }
 
-#if AP_SIM_SLUNGPAYLOAD_ENABLED
+// add body-frame force due to slung payload and tether
-// add body-frame force due to slung payload
+void Aircraft::add_external_forces(Vector3f &body_accel)
-void Aircraft::add_slungpayload_forces(Vector3f &body_accel)
 {
-    Vector3f forces_ef;
+    Vector3f total_force;
-    sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef);
+#if AP_SIM_SLUNGPAYLOAD_ENABLED
+    Vector3f forces_ef_slung;
+    sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef_slung);
+    total_force += forces_ef_slung;
+#endif
+
+#if AP_SIM_TETHER_ENABLED
+    Vector3f forces_ef_tether;
+    sitl->models.tether_sim.get_forces_on_vehicle(forces_ef_tether);
+    total_force += forces_ef_tether;
+#endif
 
     // convert ef forces to body-frame accelerations (acceleration = force / mass)
-    const Vector3f accel_bf = dcm.transposed() * forces_ef / mass;
+    const Vector3f accel_bf_tether = dcm.transposed() * total_force / mass;
-    body_accel += accel_bf;
+    body_accel += accel_bf_tether;
 }
-#endif
 
 /*
   get position relative to home

libraries/SITL/SIM_Aircraft.h

@@ -322,10 +322,8 @@ protected:
     void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel);
     void add_twist_forces(Vector3f &rot_accel);
 
-#if AP_SIM_SLUNGPAYLOAD_ENABLED
+    // add body-frame force due to payload
-    // add body-frame force due to slung payload
+    void add_external_forces(Vector3f &body_accel);
-    void add_slungpayload_forces(Vector3f &body_accel);
-#endif
 
     // get local thermal updraft
     float get_local_updraft(const Vector3d &currentPos);

libraries/SITL/SIM_Multicopter.cpp

@@ -49,10 +49,8 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot
     add_shove_forces(rot_accel, body_accel);
     add_twist_forces(rot_accel);
 
-#if AP_SIM_SLUNGPAYLOAD_ENABLED
+    // add forces from slung payload or tether payload
-    // add forces from slung payload
+    add_external_forces(body_accel);
-    add_slungpayload_forces(body_accel);
-#endif
 }
 
 /*

libraries/SITL/SIM_Tether.cpp

@@ -0,0 +1,309 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulate a static tether attached to the vehicle and ground
+*/
+
+#include "SIM_config.h"
+
+#if AP_SIM_TETHER_ENABLED
+
+#include "SIM_Tether.h"
+#include "SITL.h"
+#include <stdio.h>
+#include "SIM_Aircraft.h"
+#include <AP_HAL_SITL/SITL_State.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_Logger/AP_Logger.h>
+
+using namespace SITL;
+
+// TetherSim parameters
+const AP_Param::GroupInfo TetherSim::var_info[] = {
+    // @Param: ENABLE
+    // @DisplayName: Tether Simulation Enable/Disable
+    // @Description: Enable or disable the tether simulation
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("ENABLE",  1, TetherSim,  enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: DENSITY
+    // @DisplayName: Tether Wire Density
+    // @Description: Linear mass density of the tether wire
+    // @Range: 0 1
+    // @User: Advanced
+    AP_GROUPINFO("DENSITY",  2, TetherSim,  tether_linear_density, 0.0167),
+
+    // @Param: LINELEN
+    // @DisplayName: Tether Maximum Line Length
+    // @Description: Maximum length of the tether line in meters
+    // @Units: m
+    // @Range: 0 100
+    // @User: Advanced
+    AP_GROUPINFO("LINELEN", 3, TetherSim,  max_line_length, 100.0),
+
+    // @Param: SYSID
+    // @DisplayName: Tether Simulation MAVLink System ID
+    // @Description: MAVLink system ID for the tether simulation, used to distinguish it from other systems on the network
+    // @Range: 0 255
+    // @User: Advanced
+    AP_GROUPINFO("SYSID",   4, TetherSim,  sys_id, 2),
+
+    // @Param: STUCK
+    // @DisplayName: Tether Stuck Enable/Disable
+    // @Description: Enable or disable a stuck tether simulation
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("STUCK",   5, TetherSim,  tether_stuck, 0),
+
+    // @Param: SPGCNST
+    // @DisplayName: Tether Spring Constant
+    // @Description: Spring constant for the tether to simulate elastic forces when stretched beyond its maximum length
+    // @Range: 0 255
+    // @User: Advanced
+    AP_GROUPINFO("SPGCNST",   6, TetherSim,  tether_spring_constant, 100),
+
+    AP_GROUPEND
+};
+
+// TetherSim handles interaction with main vehicle
+TetherSim::TetherSim()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+
+void TetherSim::update(const Location& veh_pos)
+{
+    if (!enable) {
+        return;
+    }
+
+    if (veh_pos.is_zero()) {
+        return;
+    }
+
+    // initialise fixed tether ground location
+    const uint32_t now_us = AP_HAL::micros();
+    if (!initialised) {
+        // capture EKF origin
+        auto *sitl = AP::sitl();
+        const Location ekf_origin = sitl->state.home;
+        if (ekf_origin.lat == 0 && ekf_origin.lng == 0) {
+            return;
+        }
+
+        // more initialisation
+        last_update_us = now_us;
+        initialised = true;
+    }
+
+    // calculate dt and update tether forces
+    const float dt = (now_us - last_update_us)*1.0e-6;
+    last_update_us = now_us;
+
+    update_tether_force(veh_pos, dt);
+
+    // send tether location to GCS at 5hz
+    // TO-Do: add provision to make the tether movable
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_report_ms >= reporting_period_ms) {
+        last_report_ms = now_ms;
+        send_report();
+        write_log();
+    }
+}
+
+// get earth-frame forces on the vehicle from the tether
+// returns true on success and fills in forces_ef argument, false on failure
+bool TetherSim::get_forces_on_vehicle(Vector3f& forces_ef) const
+{
+    if (!enable) {
+        return false;
+    }
+
+    forces_ef = veh_forces_ef;
+    return true;
+}
+
+// send a report to the vehicle control code over MAVLink
+void TetherSim::send_report(void)
+{
+    if (!mavlink_connected && mav_socket.connect(target_address, target_port)) {
+        ::printf("Tether System connected to %s:%u\n", target_address, (unsigned)target_port);
+        mavlink_connected = true;
+    }
+    if (!mavlink_connected) {
+        return;
+    }
+
+    // get current time
+    uint32_t now_ms = AP_HAL::millis();
+
+    // send heartbeat at 1hz
+    const uint8_t component_id = MAV_COMP_ID_USER11;
+    if (now_ms - last_heartbeat_ms >= 1000) {
+        last_heartbeat_ms = now_ms;
+
+        const mavlink_heartbeat_t heartbeat{
+            custom_mode: 0,
+            type : MAV_TYPE_GROUND_ROVER,
+            autopilot : MAV_AUTOPILOT_INVALID,
+            base_mode: 0,
+            system_status: 0,
+            mavlink_version: 0,
+        };
+
+        mavlink_message_t msg;
+        mavlink_msg_heartbeat_encode_status(
+            sys_id.get(),
+            component_id,
+            &mav_status,
+            &msg,
+            &heartbeat);
+        uint8_t buf[300];
+        const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+        mav_socket.send(buf, len);
+    }
+
+    // send a GLOBAL_POSITION_INT messages
+    {
+        Location tether_anchor_loc;
+        int32_t alt_amsl_cm, alt_rel_cm;
+        if (!get_tether_ground_location(tether_anchor_loc) ||
+            !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) ||
+            !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) {
+            return;
+        }
+        const mavlink_global_position_int_t global_position_int{
+            time_boot_ms: now_ms,
+            lat: tether_anchor_loc.lat,
+            lon: tether_anchor_loc.lng,
+            alt: alt_amsl_cm * 10,              // amsl alt in mm
+            relative_alt: alt_rel_cm * 10,      // relative alt in mm
+            vx: 0,  // velocity in cm/s
+            vy: 0,  // velocity in cm/s
+            vz: 0,  // velocity in cm/s
+            hdg: 0                              // heading in centi-degrees
+        };
+        mavlink_message_t msg;
+        mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int);
+        uint8_t buf[300];
+        const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+        if (len > 0) {
+            mav_socket.send(buf, len);
+        }
+    }
+}
+
+void TetherSim::write_log()
+{
+#if HAL_LOGGING_ENABLED
+    // write log of tether state
+    // @LoggerMessage: TETH
+    // @Description: Tether state
+    // @Field: TimeUS: Time since system startup
+    // @Field: Len: Tether length
+    // @Field: VFN: Force on vehicle in North direction
+    // @Field: VFE: Force on vehicle in East direction
+    // @Field: VFD: Force on vehicle in Down direction
+    AP::logger().WriteStreaming("TETH",
+                                "TimeUS,Len,VFN,VFE,VFD",  // labels
+                                "s----",  // units
+                                "F----",    // multipliers
+                                "Qffff",     // format
+                                AP_HAL::micros64(),
+                                (float)tether_length,
+                                (double)veh_forces_ef.x,
+                                (double)veh_forces_ef.y,
+                                (double)veh_forces_ef.z);
+#endif
+}
+// returns true on success and fills in tether_ground_loc argument, false on failure
+bool TetherSim::get_tether_ground_location(Location& tether_ground_loc) const
+{
+    // get EKF origin
+    auto *sitl = AP::sitl();
+    if (sitl == nullptr) {
+        return false;
+    }
+    const Location ekf_origin = sitl->state.home;
+    if (ekf_origin.lat == 0 && ekf_origin.lng == 0) {
+        return false;
+    }
+
+    tether_ground_loc = ekf_origin;
+    return true;
+}
+
+void TetherSim::update_tether_force(const Location& veh_pos, float dt)
+{
+
+    Location tether_anchor_loc;
+    if (!get_tether_ground_location(tether_anchor_loc)) {
+        return;
+    }
+
+    // Step 1: Calculate the vector from the tether anchor to the vehicle
+    Vector3f tether_vector = veh_pos.get_distance_NED(tether_anchor_loc);
+    tether_length = tether_vector.length();
+
+    // Step 2: Check if tether is taut (length exceeds maximum allowed length) or stuck
+    if (tether_length > max_line_length) {
+
+        // 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;
+
+        // Direction of force is along the tether, pulling toward the anchor
+        veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude;
+
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Exceeded maximum length.");
+
+        return;
+    }
+
+    if (tether_stuck) {
+
+        // Calculate the stretch beyond the maximum length
+        float stretch = MAX(tether_length - tether_not_stuck_length, 0.0f);
+
+        // Apply a spring-like penalty force proportional to the stretch
+        float penalty_force_magnitude = tether_spring_constant * stretch;
+
+        // Direction of force is directly along the tether, towards the tether anchor point
+        veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude;
+
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Stuck.");
+
+        return;
+    } else {
+        tether_not_stuck_length = tether_length;
+    }
+
+    // Step 3: Calculate the weight of the tether being lifted
+    // The weight is proportional to the current tether length
+    const float tether_weight_force = tether_linear_density * tether_length * GRAVITY_MSS;
+
+    // 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;
+}
+
+#endif

libraries/SITL/SIM_Tether.h

@@ -0,0 +1,95 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+   Simulate a tethered vehicle.
+   Models the forces exerted by the tether and reports them to the vehicle simulation. The dynamics are not accurate but provide a very rough approximation intended to test a "stuck tether".
+ */
+
+#pragma once
+
+#include "SIM_config.h"
+
+#if AP_SIM_TETHER_ENABLED
+
+#include <AP_HAL/utility/Socket_native.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Common/Location.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+
+namespace SITL {
+
+// TetherSim simulates a tethered to a vehicle
+class TetherSim {
+public:
+    // constructor
+    TetherSim();
+
+    // Update the tether simulation state using the vehicle's earth-frame position
+    void update(const Location& veh_pos);
+
+    // Retrieve earth-frame forces on the vehicle due to the tether
+    // Returns true on success and fills in the forces_ef argument; false on failure
+    bool get_forces_on_vehicle(Vector3f& forces_ef) const;
+
+    // Parameter table for configuration
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    // Parameters
+    AP_Int8 enable;                  // Enable or disable the tether simulation
+    AP_Float tether_linear_density;  // Linear mass density of the tether in kg/m
+    AP_Float max_line_length;        // Maximum allowed tether length in meters
+    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
+
+    // Send MAVLink messages to the GCS
+    void send_report();
+
+    // Write tether simulation state to onboard log
+    void write_log();
+
+    // Retrieve the location of the tether anchor point on the ground
+    // Returns true on success and fills in the tether_anchor_loc argument; false on failure
+    bool get_tether_ground_location(Location& tether_anchor_loc) const;
+
+    // Update forces exerted by the tether based on the vehicle's position
+    // Takes the vehicle's position and the simulation time step (dt) as inputs
+    void update_tether_force(const Location& veh_pos, float dt);
+
+    // Socket connection variables for MAVLink communication
+    const char *target_address = "127.0.0.1"; // Address for MAVLink socket communication
+    const uint16_t target_port = 5763;        // Port for MAVLink socket communication
+    SocketAPM_native mav_socket { false };   // Socket for MAVLink communication
+    bool initialised;                        // True if the simulation class is initialized
+    uint32_t last_update_us;                 // Timestamp of the last update in microseconds
+
+    // MAVLink reporting variables
+    const float reporting_period_ms = 100;   // Reporting period in milliseconds
+    uint32_t last_report_ms;                // Timestamp of the last MAVLink report sent to GCS
+    uint32_t last_heartbeat_ms;             // Timestamp of the last MAVLink heartbeat sent to GCS
+    bool mavlink_connected;                 // True if MAVLink connection is established
+    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 tether_length = 0.0f;               // Current tether length in meters
+    float tether_not_stuck_length = 0.0f;     // Tether length when the tether is not stuck
+};
+
+}  // namespace SITL
+
+#endif  // AP_SIM_TETHER_ENABLED

libraries/SITL/SIM_config.h

@@ -57,6 +57,10 @@
 #define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
 
+#ifndef AP_SIM_TETHER_ENABLED
+#define AP_SIM_TETHER_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif
+
 #ifndef AP_SIM_FLIGHTAXIS_ENABLED
 #define AP_SIM_FLIGHTAXIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif

libraries/SITL/SITL.cpp

@@ -1367,6 +1367,12 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = {
     AP_SUBGROUPPTR(flightaxis_ptr, "RFL_", 5, SIM::ModelParm, FlightAxis),
 #endif
 
+#if AP_SIM_TETHER_ENABLED
+    // @Group: TETH_
+    // @Path: ./SIM_Tether.cpp
+    AP_SUBGROUPINFO(tether_sim, "TETH_", 6, SIM::ModelParm, TetherSim),
+#endif
+
     AP_GROUPEND
 };
     

libraries/SITL/SITL.h

@@ -28,6 +28,7 @@
 #include "SIM_IntelligentEnergy24.h"
 #include "SIM_Ship.h"
 #include "SIM_SlungPayload.h"
+#include "SIM_Tether.h"
 #include "SIM_GPS.h"
 #include "SIM_DroneCANDevice.h"
 #include "SIM_ADSB_Sagetech_MXS.h"
@@ -338,6 +339,9 @@ public:
 #if AP_SIM_SLUNGPAYLOAD_ENABLED
         SlungPayloadSim slung_payload_sim;
 #endif
+#if AP_SIM_TETHER_ENABLED
+        TetherSim tether_sim;
+#endif
 #if AP_SIM_FLIGHTAXIS_ENABLED
         FlightAxis *flightaxis_ptr;
 #endif