diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 3e519576e4317..9ca985a14591a 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_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;