SITL: integrate SlungPayload

ArduPilot · Jul 24, 2024 · a1579bc · a1579bc
1 parent 5c2b758
commit a1579bc
Show file tree

Hide file tree

Showing 6 changed files with 42 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;