From 1e2340fbb46ec495e2a250928b8abc5830f703be Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 5 Aug 2019 13:49:28 -0300 Subject: [PATCH] SITL: Fix buoyancy direction for earth-frame MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Previously the sub would just go upwards, so rolling it 180ยบ caused it to sink --- libraries/SITL/SIM_Submarine.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp index 2f9c6f2f39..a15358ec7f 100644 --- a/libraries/SITL/SIM_Submarine.cpp +++ b/libraries/SITL/SIM_Submarine.cpp @@ -70,7 +70,7 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a rot_accel = Vector3f(0,0,0); // slight positive buoyancy - body_accel = Vector3f(0, 0, -calculate_buoyancy_acceleration()); + body_accel = dcm.transposed() * Vector3f(0, 0, -calculate_buoyancy_acceleration()); for (int i = 0; i < n_thrusters; i++) { Thruster t = thrusters[i];