diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index a6baf91eb9da2..05493e9506157 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -796,6 +796,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
}
}
+ // update slung payload
+#if AP_SIM_SLUNGPAYLOAD_ENABLED
+ sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth);
+#endif
+
// allow for changes in physics step
adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
}
@@ -1227,6 +1232,19 @@ 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)
+{
+ Vector3f forces_ef;
+ sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef);
+
+ // convert ef forces to body-frame accelerations (acceleration = force / mass)
+ const Vector3f accel_bf = dcm.transposed() * forces_ef / mass;
+ body_accel += accel_bf;
+}
+#endif
+
/*
get position relative to home
*/
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 90f07089ea60b..e84a078ac8faf 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -322,6 +322,11 @@ class Aircraft {
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
+
// 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 a866762593e05..4e019dde9b4de 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -48,6 +48,11 @@ 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
}
/*
diff --git a/libraries/SITL/SIM_SlungPayload.cpp b/libraries/SITL/SIM_SlungPayload.cpp
new file mode 100644
index 0000000000000..03b5363300d7c
--- /dev/null
+++ b/libraries/SITL/SIM_SlungPayload.cpp
@@ -0,0 +1,458 @@
+/*
+ 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 slung payload
+*/
+
+#include "SIM_config.h"
+
+#if AP_SIM_SLUNGPAYLOAD_ENABLED
+
+#include "SIM_SlungPayload.h"
+#include "SITL.h"
+#include
+#include "SIM_Aircraft.h"
+#include
+#include
+#include
+
+using namespace SITL;
+
+// SlungPayloadSim parameters
+const AP_Param::GroupInfo SlungPayloadSim::var_info[] = {
+ // @Param: ENABLE
+ // @DisplayName: Slung Payload Sim enable/disable
+ // @Description: Slung Payload Sim enable/disable
+ // @Values: 0:Disabled,1:Enabled
+ // @User: Advanced
+ AP_GROUPINFO_FLAGS("ENABLE", 1, SlungPayloadSim, enable, 0, AP_PARAM_FLAG_ENABLE),
+
+ // @Param: WEIGHT
+ // @DisplayName: Slung Payload weight
+ // @Description: Slung Payload weight in kg
+ // @Units: kg
+ // @Range: 0 15
+ // @User: Advanced
+ AP_GROUPINFO("WEIGHT", 2, SlungPayloadSim, weight_kg, 1.0),
+
+ // @Param: LINELEN
+ // @DisplayName: Slung Payload line length
+ // @Description: Slung Payload line length in meters
+ // @Units: m
+ // @Range: 0 100
+ // @User: Advanced
+ AP_GROUPINFO("LINELEN", 3, SlungPayloadSim, line_length, 30.0),
+
+ // @Param: DRAG
+ // @DisplayName: Slung Payload drag coefficient
+ // @Description: Slung Payload drag coefficient. Higher values increase drag and slow the payload more quickly
+ // @Units: m
+ // @Range: 0 10
+ // @User: Advanced
+ AP_GROUPINFO("DRAG", 4, SlungPayloadSim, drag_coef, 1),
+
+ // @Param: SYSID
+ // @DisplayName: Slung Payload MAVLink system ID
+ // @Description: Slung Payload MAVLink system id to distinguish it from others on the same network
+ // @Range: 0 255
+ // @User: Advanced
+ AP_GROUPINFO("SYSID", 5, SlungPayloadSim, sys_id, 2),
+
+ AP_GROUPEND
+};
+
+// SlungPayloadSim handles interaction with main vehicle
+SlungPayloadSim::SlungPayloadSim()
+{
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+// update the SlungPayloadSim's state using the vehicle's earth-frame position, velocity and acceleration
+void SlungPayloadSim::update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef)
+{
+ if (!enable) {
+ return;
+ }
+
+ // initialise slung payload 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 slung payload
+ const float dt = (now_us - last_update_us)*1.0e-6;
+ last_update_us = now_us;
+ update_payload(veh_pos, veh_vel_ef, veh_accel_ef, dt);
+
+ // send payload location to GCS at 5hz
+ 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 slung payload
+// returns true on success and fills in forces_ef argument, false on failure
+bool SlungPayloadSim::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 SlungPayloadSim::send_report(void)
+{
+ if (!mavlink_connected && mav_socket.connect(target_address, target_port)) {
+ ::printf("SlungPayloadSim 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_AIRSHIP,
+ 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 payload_loc;
+ int32_t alt_amsl_cm, alt_rel_cm;
+ if (!get_payload_location(payload_loc) ||
+ !payload_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) ||
+ !payload_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: payload_loc.lat,
+ lon: payload_loc.lng,
+ alt: alt_amsl_cm * 10, // amsl alt in mm
+ relative_alt: alt_rel_cm * 10, // relative alt in mm
+ vx: int16_t(velocity_NED.x * 100), // velocity in cm/s
+ vy: int16_t(velocity_NED.y * 100), // velocity in cm/s
+ vz: int16_t(velocity_NED.z * 100), // 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);
+ }
+ }
+
+ // send ATTITUDE so MissionPlanner can display orientation
+ {
+ const mavlink_attitude_t attitude{
+ time_boot_ms: now_ms,
+ roll: 0,
+ pitch: 0,
+ yaw: 0, // heading in radians
+ rollspeed: 0,
+ pitchspeed: 0,
+ yawspeed: 0
+ };
+ mavlink_message_t msg;
+ mavlink_msg_attitude_encode_status(
+ sys_id,
+ component_id,
+ &mav_status,
+ &msg,
+ &attitude);
+ uint8_t buf[300];
+ const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+ if (len > 0) {
+ mav_socket.send(buf, len);
+ }
+ }
+}
+
+// write onboard log
+void SlungPayloadSim::write_log()
+{
+#if HAL_LOGGING_ENABLED
+ // write log of slung payload state
+ // @LoggerMessage: SLUP
+ // @Description: Slung payload
+ // @Field: TimeUS: Time since system startup
+ // @Field: Land: 1 if payload is landed, 0 otherwise
+ // @Field: Tens: Tension ratio, 1 if line is taut, 0 if slack
+ // @Field: Len: Line length
+ // @Field: PN: Payload position as offset from vehicle in North direction
+ // @Field: PE: Payload position as offset from vehicle in East direction
+ // @Field: PD: Payload position as offset from vehicle in Down direction
+ // @Field: VN: Payload velocity in North direction
+ // @Field: VE: Payload velocity in East direction
+ // @Field: VD: Payload velocity in Down direction
+ // @Field: AN: Payload acceleration in North direction
+ // @Field: AE: Payload acceleration in East direction
+ // @Field: AD: Payload acceleration in Down direction
+ // @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("SLUP",
+ "TimeUS,Land,Tens,Len,PN,PE,PD,VN,VE,VD,AN,AE,AD,VFN,VFE,VFD", // labels
+ "s-%mmmmnnnooo---", // units
+ "F-20000000000000", // multipliers
+ "Qbffffffffffffff", // format
+ AP_HAL::micros64(),
+ (uint8_t)landed,
+ (float)tension_ratio,
+ (float)payload_to_veh.length(),
+ (double)-payload_to_veh.x,
+ (double)-payload_to_veh.y,
+ (double)-payload_to_veh.z,
+ (double)velocity_NED.x,
+ (double)velocity_NED.y,
+ (double)velocity_NED.z,
+ (double)accel_NED.x,
+ (double)accel_NED.y,
+ (double)accel_NED.z,
+ (double)veh_forces_ef.x,
+ (double)veh_forces_ef.y,
+ (double)veh_forces_ef.z);
+#endif
+}
+
+// returns true on success and fills in payload_loc argument, false on failure
+bool SlungPayloadSim::get_payload_location(Location& payload_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;
+ }
+
+ // calculate location
+ payload_loc = ekf_origin;
+ payload_loc.offset(position_NED);
+ return true;
+}
+
+// update the slung payloads position, velocity, acceleration
+// vehicle position, velocity and acceleration should be in earth-frame NED frame
+void SlungPayloadSim::update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, float dt)
+{
+ // how we calculate the payload's position, velocity and acceleration
+ // 1. update the payload's position, velocity using the previous iterations acceleration
+ // 2. check that the payload does not fall below the terrain
+ // 3. check if the line is taught and that the payload does not move more than the line length from the vehicle
+ // 4. calculate gravity and drag forces on the payload
+ // 5. calculate the tension force between the payload and vehicle including force countering gravity, drag and centripetal force
+ // 6. update the payload's acceleration using the sum of the above forces
+
+ // initialise position_NED from vehicle position
+ if (position_NED.is_zero()) {
+ if (!veh_pos.is_zero()) {
+ position_NED = veh_pos;
+ GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: initialised at %f %f %f", position_NED.x, position_NED.y, position_NED.z);
+ }
+ return;
+ }
+
+ // integrate previous iterations acceleration into velocity and position
+ velocity_NED += accel_NED * dt;
+ position_NED += (velocity_NED * dt).todouble();
+
+ // calculate distance from payload to vehicle
+ payload_to_veh = veh_pos - position_NED;
+ float payload_to_veh_length = payload_to_veh.length();
+
+ // update landed state by checking if payload has dropped below terrain
+ Location payload_loc;
+ if (get_payload_location(payload_loc)) {
+ int32_t alt_terrain_cm;
+ bool landed_orig = landed;
+ if (payload_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_terrain_cm)) {
+
+ // landed if below terrain
+ if (alt_terrain_cm <= 0) {
+ landed = true;
+
+ // raise payload to match terrain
+ position_NED.z += (alt_terrain_cm * 0.01);
+
+ // zero out velocity and acceleration in horizontal and downward direction
+ velocity_NED.xy().zero();
+ velocity_NED.z = MIN(velocity_NED.z, 0);
+ accel_NED.xy().zero();
+ accel_NED.z = MIN(accel_NED.z, 0);
+
+ // zero out forces on vehicle
+ veh_forces_ef.zero();
+ }
+
+ // not landed if above terrain
+ if (landed && (alt_terrain_cm > 1)) {
+ landed = false;
+ }
+ }
+
+ // inform user if landed state has changed
+ if (landed != landed_orig) {
+ if (landed) {
+ // get payload location again in case it has moved
+ get_payload_location(payload_loc);
+ GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: landed lat:%f lon:%f alt:%4.1f",
+ (double)payload_loc.lat * 1e-7,
+ (double)payload_loc.lng * 1e-7,
+ (double)payload_loc.alt * 1e-2);
+ } else {
+ GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: liftoff");
+ }
+ }
+ }
+
+ // calculate forces of gravity
+ Vector3f force_gravity_NED = Vector3f(0.0f, 0.0f, GRAVITY_MSS * weight_kg);
+
+ // tension force on payload (resists gravity, drag, centripetal force)
+ Vector3f tension_force_NED;
+
+ // tension ratio to smooth transition from line being taut to slack
+ tension_ratio = 0;
+
+ // calculate drag force (0.5 * drag_coef * air_density * velocity^2 * surface area)
+ Vector3f force_drag_NED;
+ if (drag_coef > 0 && !velocity_NED.is_zero()) {
+ const float air_density = 1.225; // 1.225 kg/m^3 (standard sea-level density)
+ const float surface_area_m2 = 0.07; // 30cm diameter sphere
+ const float drag_force = 0.5 * drag_coef * air_density * velocity_NED.length_squared() * surface_area_m2;
+ force_drag_NED = -velocity_NED.normalized() * drag_force;
+ }
+
+ // sanity check payload distance from vehicle and calculate tension force
+ if (is_positive(payload_to_veh_length)) {
+
+ // calculate unit vector from payload to vehicle
+ const Vector3f payload_to_veh_norm = payload_to_veh.normalized().tofloat();
+
+ // ensure payload is no more than line_length from vehicle
+ if (payload_to_veh_length > line_length) {
+ payload_to_veh *= (line_length / payload_to_veh_length);
+ position_NED = veh_pos - payload_to_veh;
+ }
+
+ // calculate tension ratio as value between 0 and 1
+ // tension ratio is 0 when payload-to-vehicle distance is 10cm less than line length
+ // tension ratio is 1 when payload-to-vehicle distance is equal to line length
+ tension_ratio = constrain_float(1.0 - (line_length - payload_to_veh_length) * 10, 0, 1);
+
+ // calculate tension forces when line is taut
+ if (is_positive(tension_ratio)) {
+
+ // tension resists gravity if vehicle is above payload
+ if (is_negative(payload_to_veh_norm.z)) {
+ tension_force_NED += -force_gravity_NED.projected(payload_to_veh_norm);
+ }
+
+ // calculate tension force resulting from velocity difference between vehicle and payload
+ // use time constant to convert velocity to acceleration
+ const float velocity_to_accel_TC = 2.0;
+ Vector3f velocity_diff_NED = (veh_vel_ef - velocity_NED).projected(payload_to_veh_norm);
+
+ // add to tension force if the vehicle is moving faster than the payload
+ if (vectors_same_direction(velocity_diff_NED, payload_to_veh_norm)) {
+ tension_force_NED += velocity_diff_NED / velocity_to_accel_TC * weight_kg;
+ }
+
+ // tension force resisting payload drag
+ tension_force_NED += -force_drag_NED.projected(payload_to_veh_norm);
+
+ // calculate centripetal force
+ const Vector3f velocity_parallel = velocity_NED.projected(payload_to_veh_norm);
+ const Vector3f velocity_perpendicular = velocity_NED - velocity_parallel;
+ const float tension_force_centripetal = velocity_perpendicular.length_squared() * weight_kg / line_length;
+ const Vector3f tension_force_centripetal_NED = payload_to_veh_norm * tension_force_centripetal;
+
+ // add centripetal force to tension force
+ tension_force_NED += tension_force_centripetal_NED;
+
+ // scale tension force by tension ratio
+ tension_force_NED *= tension_ratio;
+ }
+ }
+
+ // force on vehicle is opposite to tension force on payload
+ veh_forces_ef = -tension_force_NED;
+
+ // convert force to acceleration (f=m*a => a=f/m)
+ accel_NED = (force_gravity_NED + force_drag_NED + tension_force_NED) / weight_kg;
+
+ // if slung payload is landed we zero out downward (e.g positive) acceleration
+ if (landed) {
+ accel_NED.z = MIN(accel_NED.z, 0);
+ // should probably zero out forces_ef vertical component as well?
+ }
+}
+
+// returns true if the two vectors point in the same direction, false if perpendicular or opposite
+bool SlungPayloadSim::vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const
+{
+ // check both vectors are non-zero
+ if (v1.is_zero() || v2.is_zero()) {
+ return false;
+ }
+ return v1.dot(v2) > 0;
+}
+
+#endif
diff --git a/libraries/SITL/SIM_SlungPayload.h b/libraries/SITL/SIM_SlungPayload.h
new file mode 100644
index 0000000000000..3f668af547294
--- /dev/null
+++ b/libraries/SITL/SIM_SlungPayload.h
@@ -0,0 +1,102 @@
+/*
+ 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 payload slung from a line under a vehicle
+*/
+
+#pragma once
+
+#include "SIM_config.h"
+
+#if AP_SIM_SLUNGPAYLOAD_ENABLED
+
+#include
+#include
+#include
+#include
+
+namespace SITL {
+
+// SlungPayloadSim handles interaction with main vehicle
+class SlungPayloadSim {
+public:
+ friend class SlungPayload;
+
+ // constructor
+ SlungPayloadSim();
+
+ // update the SlungPayloadSim's state using thevehicle's earth-frame position, velocity and acceleration
+ void update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef);
+
+ // get earth-frame forces on the vehicle from slung payload
+ // returns true on success and fills in forces_ef argument, false on failure
+ bool get_forces_on_vehicle(Vector3f& forces_ef) const;
+
+ // parameter table
+ static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+ // parameters
+ AP_Int8 enable; // enable parameter
+ AP_Float weight_kg; // payload weight in kg
+ AP_Float line_length; // line length in meters
+ AP_Int8 sys_id; // mavlink system id for reporting to GCS
+ AP_Float drag_coef; // drag coefficient (spheres=0.5, cubes=1.05, barrels=0.8~1.2)
+
+ // send MAVLink messages to GCS
+ void send_report();
+
+ // write onboard log
+ void write_log();
+
+ // get payload location
+ // returns true on success and fills in payload_loc argument, false on failure
+ bool get_payload_location(Location& payload_loc) const;
+
+ // update the slung payload's position, velocity, acceleration
+ // vehicle position, velocity and acceleration should be in earth-frame NED frame
+ void update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, float dt);
+
+ // returns true if the two vectors point in the same direction, false if perpendicular or opposite
+ bool vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const;
+
+ // socket connection variables
+ const char *target_address = "127.0.0.1";
+ const uint16_t target_port = 5763;
+ SocketAPM_native mav_socket { false };
+ bool initialised; // true if this class has been initialised
+ uint32_t last_update_us; // system time of last update
+
+ // mavlink reporting variables
+ const float reporting_period_ms = 200; // reporting period in ms
+ uint32_t last_report_ms; // system time of last MAVLink report sent to GCS
+ uint32_t last_heartbeat_ms; // system time of last MAVLink heartbeat sent to GCS
+ bool mavlink_connected; // true if a mavlink connection has been established
+ mavlink_status_t mav_status; // reported mavlink status
+
+ // payload variables
+ bool landed = true; // true if the payload is on the ground
+ float tension_ratio; // 0 if line is loose, 1 if completely taut
+ Vector3p payload_to_veh;// distance vector (in meters in NED frame) from payload to vehicle (used for reporting purposes)
+ Vector3p position_NED; // payload's position (as an offset from EKF origin? offset from vehicle?) in meters
+ Vector3f velocity_NED; // payload velocity
+ Vector3f accel_NED; // payload's acceleration
+ Vector3f veh_forces_ef; // earth-frame forces on the vehicle caused by the payload
+};
+
+} // namespace SITL
+
+#endif // AP_SIM_SLUNGPAYLOAD_ENABLED
diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h
index c83e83d44767d..8ec2d7ff42c48 100644
--- a/libraries/SITL/SIM_config.h
+++ b/libraries/SITL/SIM_config.h
@@ -51,6 +51,10 @@
#define AP_SIM_SHIP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
+#ifndef AP_SIM_SLUNGPAYLOAD_ENABLED
+#define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif
+
#ifndef AP_SIM_TSYS03_ENABLED
#define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index a21e744c349b5..681d5d3dbf440 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -1249,6 +1249,12 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = {
AP_SUBGROUPPTR(glider_ptr, "GLD_", 3, SIM::ModelParm, Glider),
#endif
+#if AP_SIM_SLUNGPAYLOAD_ENABLED
+ // @Group: SLUP_
+ // @Path: ./SIM_SlungPayload.cpp
+ AP_SUBGROUPINFO(slung_payload_sim, "SLUP_", 4, SIM::ModelParm, SlungPayloadSim),
+#endif
+
AP_GROUPEND
};
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 3b197fe8469e5..d3ad9ebbd962a 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -27,6 +27,7 @@
#include "SIM_FETtecOneWireESC.h"
#include "SIM_IntelligentEnergy24.h"
#include "SIM_Ship.h"
+#include "SIM_SlungPayload.h"
#include "SIM_GPS.h"
#include "SIM_DroneCANDevice.h"
#include "SIM_ADSB_Sagetech_MXS.h"
@@ -322,6 +323,9 @@ class SIM {
#endif
#if AP_SIM_GLIDER_ENABLED
Glider *glider_ptr;
+#endif
+#if AP_SIM_SLUNGPAYLOAD_ENABLED
+ SlungPayloadSim slung_payload_sim;
#endif
};
ModelParm models;