diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index c0469abc7c..138e5ae875 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/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
-void Aircraft::add_slungpayload_forces(Vector3f &body_accel)
+// add body-frame force due to slung payload and tether
+void Aircraft::add_external_forces(Vector3f &body_accel)
{
- Vector3f forces_ef;
- sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef);
+ Vector3f total_force;
+#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;
- body_accel += accel_bf;
+ const Vector3f accel_bf_tether = dcm.transposed() * total_force / mass;
+ body_accel += accel_bf_tether;
}
-#endif
/*
get position relative to home
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index e84a078ac8..a6d7b273c0 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/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 slung payload
- void add_slungpayload_forces(Vector3f &body_accel);
-#endif
+ // add body-frame force due to payload
+ void add_external_forces(Vector3f &body_accel);
// get local thermal updraft
float get_local_updraft(const Vector3d ¤tPos);
diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index 4e019dde9b..273c2fc06d 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -49,12 +49,10 @@ 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
- add_slungpayload_forces(body_accel);
-#endif
+ // add forces from slung payload or tether payload
+ add_external_forces(body_accel);
}
-
+
/*
update the multicopter simulation by one time step
*/
diff --git a/libraries/SITL/SIM_Tether.cpp b/libraries/SITL/SIM_Tether.cpp
new file mode 100644
index 0000000000..005347b4c2
--- /dev/null
+++ b/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 .
+ */
+/*
+ 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
+#include "SIM_Aircraft.h"
+#include
+#include
+#include
+
+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
diff --git a/libraries/SITL/SIM_Tether.h b/libraries/SITL/SIM_Tether.h
new file mode 100644
index 0000000000..4a7366e108
--- /dev/null
+++ b/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 .
+ */
+
+/*
+ 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
+#include
+#include
+#include
+
+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
diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h
index d417577249..12fca3b307 100644
--- a/libraries/SITL/SIM_config.h
+++ b/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
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index e329ed232a..03ba5f1771 100644
--- a/libraries/SITL/SITL.cpp
+++ b/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
};
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 4edda5efd2..54d9350437 100644
--- a/libraries/SITL/SITL.h
+++ b/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