From 393b018f3111b1a278f5d6315363f5fd1e183410 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Wed, 18 Sep 2019 14:15:23 -0300
Subject: [PATCH] SITL: Sub: Fix rotational physics logic

---
 libraries/SITL/SIM_Submarine.cpp | 21 ++++++++++++++++++---
 libraries/SITL/SIM_Submarine.h   |  4 +++-
 2 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp
index 32cbf39387..5bb90eded8 100644
--- a/libraries/SITL/SIM_Submarine.cpp
+++ b/libraries/SITL/SIM_Submarine.cpp
@@ -100,14 +100,14 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
 
     // Calculate angular drag forces
     // TODO: This results in the wrong units. Fix the math.
-    Vector3f angular_drag_forces;
-    calculate_drag_force(gyro, frame_property.angular_drag_coefficient, angular_drag_forces);
+    Vector3f angular_drag_torque;
+    calculate_angular_drag_torque(gyro, frame_property.angular_drag_coefficient, angular_drag_torque);
 
     // Calculate torque induced by buoyancy foams on the frame
     Vector3f buoyancy_torque;
     calculate_buoyancy_torque(buoyancy_torque);
     // Add forces in body frame accel
-    rot_accel -= angular_drag_forces / frame_property.weight;
+    rot_accel -= angular_drag_torque / frame_property.moment_of_inertia;
     rot_accel += buoyancy_torque / frame_property.moment_of_inertia;
     add_shove_forces(rot_accel, body_accel);
 }
@@ -153,6 +153,21 @@ void Submarine::calculate_drag_force(const Vector3f &velocity, const Vector3f &d
     force *= drag_coefficient;
 }
 
+/**
+ * @brief Calculate angular drag torque
+ *
+ * @param angular_velocity Body frame velocity of fluid
+ * @param drag_coefficient Rotational drag coefficient of body
+ */
+void Submarine::calculate_angular_drag_torque(const Vector3f &angular_velocity, const Vector3f &drag_coefficient, Vector3f &torque)
+{
+    // TODO: Find a good approximation for this.
+    // While this works, it is not accurate nor has the right dimensions.
+    torque = angular_velocity;
+    torque *= drag_coefficient;
+}
+
+
 /**
 * @brief Calculate buoyancy force of the frame
 *
diff --git a/libraries/SITL/SIM_Submarine.h b/libraries/SITL/SIM_Submarine.h
index 9669f2703e..f709536ccb 100644
--- a/libraries/SITL/SIM_Submarine.h
+++ b/libraries/SITL/SIM_Submarine.h
@@ -81,7 +81,7 @@ protected:
 
         // Frame drag coefficient
         const Vector3f linear_drag_coefficient = Vector3f(0.2, 0.3, 0.4);
-        const Vector3f angular_drag_coefficient = Vector3f(1, 1, 1);
+        const Vector3f angular_drag_coefficient = Vector3f(5, 5, 5);
         // Calculate total volume from water buoyancy
         // $ V = F_b / (rho * g) $
         // V = volume (m^3), rho = water density (kg/m^3), g = gravity (m/s^2), F_b = force (N)
@@ -103,6 +103,8 @@ protected:
     float calculate_buoyancy_acceleration();
     // calculate drag from velocity and drag coefficient
     void calculate_drag_force(const Vector3f &velocity, const Vector3f &drag_coefficient, Vector3f &force);
+    // calculate torque water resistance
+    void calculate_angular_drag_torque(const Vector3f &angular_velocity, const Vector3f &drag_coefficient, Vector3f &torque);
     // calculate torque induced by buoyancy foams
     void calculate_buoyancy_torque(Vector3f &torque);