From 8bb0772224cc5ac25eb9fedb75a4af912d0fe920 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= <patrickelectric@gmail.com>
Date: Mon, 4 Mar 2019 13:18:28 -0800
Subject: [PATCH] SITL: Add drag simulation in Sub
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
---
 libraries/SITL/SIM_Submarine.cpp | 49 +++++++++++++++++++++++---------
 libraries/SITL/SIM_Submarine.h   | 17 +++++++++++
 2 files changed, 53 insertions(+), 13 deletions(-)

diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp
index 9f1152266a..7cd9f0eb06 100644
--- a/libraries/SITL/SIM_Submarine.cpp
+++ b/libraries/SITL/SIM_Submarine.cpp
@@ -70,20 +70,43 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
     	body_accel.z = -GRAVITY_MSS;
     }
 
-    float terminal_rotation_rate = 10.0;
-    if (terminal_rotation_rate > 0) {
-        // rotational air resistance
-        rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
-        rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
-        rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
-    }
+    // Calculate linear drag forces
+    Vector3f linear_drag_forces;
+    calculate_drag_force(velocity_air_bf, frame_property.linear_drag_coefficient, linear_drag_forces);
+    // Add forces in body frame accel
+    body_accel -= linear_drag_forces / frame_property.weight;
 
-    float terminal_velocity = 3.0;
-    if (terminal_velocity > 0) {
-        // air resistance
-        Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
-        body_accel += dcm.transposed() * air_resistance;
-    }
+    // Calculate angular drag forces
+    Vector3f angular_drag_forces;
+    calculate_drag_force(gyro, frame_property.angular_drag_coefficient, angular_drag_forces);
+    // Add forces in body frame accel
+    rot_accel -= angular_drag_forces / frame_property.weight;
+}
+
+/**
+ * @brief Calculate drag force against body
+ *
+ * @param velocity Body frame velocity of fluid
+ * @param drag_coefficient Drag coefficient of body
+ * @param force Output forces
+ * $ F_D = rho * v^2 * A * C_D / 2 $
+ * rho = water density (kg/m^3), V = velocity (m/s), A = area (m^2), C_D = drag_coefficient
+ */
+void Submarine::calculate_drag_force(const Vector3f &velocity, const Vector3f &drag_coefficient, Vector3f &force)
+{
+    /**
+     * @brief It's necessary to keep the velocity orientation from the body frame.
+     *     To do so, a mathematical artifice is used to do velocity square but without loosing the direction.
+     *  $(|V|/V)*V^2$ = $|V|*V$
+     */
+    const Vector3f velocity_2(
+        fabsf(velocity.x) * velocity.x,
+        fabsf(velocity.y) * velocity.y,
+        fabsf(velocity.z) * velocity.z
+    );
+
+    force = (velocity_2 * water_density) * frame_property.equivalent_sphere_area / 2.0f;
+    force *= drag_coefficient;
 }
 
 /**
diff --git a/libraries/SITL/SIM_Submarine.h b/libraries/SITL/SIM_Submarine.h
index 5bc23ada3c..e25767fcec 100644
--- a/libraries/SITL/SIM_Submarine.h
+++ b/libraries/SITL/SIM_Submarine.h
@@ -53,6 +53,21 @@ protected:
         float net_bouyancy = 2.0; // (N)
 
         float bouyancy_acceleration = GRAVITY_MSS + net_bouyancy/weight;
+
+        // Frame drag coefficient
+        const Vector3f linear_drag_coefficient = Vector3f(0.2, 0.3, 0.4);
+        const Vector3f angular_drag_coefficient = Vector3f(1, 1, 1);
+        // 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)
+        float volume = buoyancy_acceleration * weight / (GRAVITY_MSS * 1023.6f);
+        // Calculate equivalent sphere area for drag force
+        // $ A = pi * r^2 / 4 $
+        // $ V = 4 * pi * r^3 / 3 $
+        // $ r ^2 = (V * 3 / 4) ^ (2/3) $
+        // A = area (m^3), r = sphere radius (m)
+        float equivalent_sphere_area = M_PI_4 * pow(volume * 3.0f / 4.0f, 2.0f / 3.0f);
+
     } frame_property;
 
     bool on_ground() const override;
@@ -61,6 +76,8 @@ protected:
     void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
     // calculate buoyancy
     float calculate_buoyancy_acceleration();
+    // calculate drag from velocity and drag coefficient
+    void calculate_drag_force(const Vector3f &velocity, const Vector3f &drag_coefficient, Vector3f &force);
 
     Frame *frame;
 };