From 1707390194979ac0dc8b9c73c58ae084ac9b0eaf 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      |  3 +++
 libraries/SITL/SIM_Multicopter.cpp |  3 +++
 libraries/SITL/SIM_config.h        |  4 ++++
 libraries/SITL/SITL.cpp            |  6 ++++++
 libraries/SITL/SITL.h              |  4 ++++
 6 files changed, 38 insertions(+)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 366ec909304ace..b1ada27057917b 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));
 }
@@ -1224,6 +1229,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 4af43bb80cd092..6dc874ff1203bd 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -321,6 +321,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 &currentPos);
 
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 99a53bfb0fd437..6747ec5d71ec11 100644
--- a/libraries/SITL/SIM_config.h
+++ b/libraries/SITL/SIM_config.h
@@ -47,6 +47,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 a86fa396df622a..a4caee63f491ed 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 fdf3bddf5dad88..b2d5f4dd77ec39 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"
@@ -321,6 +322,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;