From a1579bc31e9a50a4b427e7b18791808cb650cbbd Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 12 Jul 2024 15:18:16 +0900
Subject: [PATCH] SITL: integrate SlungPayload

---
 libraries/SITL/SIM_Aircraft.cpp    | 18 ++++++++++++++++++
 libraries/SITL/SIM_Aircraft.h      |  5 +++++
 libraries/SITL/SIM_Multicopter.cpp |  5 +++++
 libraries/SITL/SIM_config.h        |  4 ++++
 libraries/SITL/SITL.cpp            |  6 ++++++
 libraries/SITL/SITL.h              |  4 ++++
 6 files changed, 42 insertions(+)

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 &currentPos);
 
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;