SITL: add simulated slung payload

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
  */

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

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
 }
 
 /*