diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index a6baf91eb9da24..3a9ddc04ba9f9e 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) } } +// add body-frame force due to slung payload +void Aircraft::add_slungpayload_forces(Vector3f &body_accel) +{ +#if AP_SIM_SLUNGPAYLOAD_ENABLED + 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 90f07089ea60b1..a850e4df932303 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -322,6 +322,9 @@ class Aircraft { void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel); void add_twist_forces(Vector3f &rot_accel); + // add body-frame force due to slung payload + void add_slungpayload_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 a866762593e05d..57987a1d88dfa1 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -48,6 +48,9 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot add_shove_forces(rot_accel, body_accel); add_twist_forces(rot_accel); + + // add forces from slung payload + add_slungpayload_forces(body_accel); } /* diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index bd5d07e799c50f..052b68b5892341 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 49c344b6e7fb0d..b127a2dbc205ca 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -1233,6 +1233,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 3b197fe8469e52..d3ad9ebbd962aa 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;