From 28ff272017474c77fbfcb16b1c6f35532e160a89 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= <patrickelectric@gmail.com>
Date: Wed, 22 Nov 2017 14:09:43 -0200
Subject: [PATCH] SIM_Submarine: Solve problem when frame is above water level
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Fix bluerobotics/ardusub/issues/128

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
Co-Authored-By: jaxxzer <jwalser90@gmail.com>
---
 libraries/SITL/SIM_Submarine.cpp | 30 ++++++++++++++++++++++++------
 libraries/SITL/SIM_Submarine.h   | 14 ++++++++++++++
 2 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp
index 5668f7d9e0..d512cec791 100644
--- a/libraries/SITL/SIM_Submarine.cpp
+++ b/libraries/SITL/SIM_Submarine.cpp
@@ -49,7 +49,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,-GRAVITY_MSS * 1.1);
+    body_accel = Vector3f(0, 0, -calculate_buoyancy_acceleration());
 
     for (int i = 0; i < 6; i++) {
         Thruster t = vectored_thrusters[i];
@@ -65,11 +65,6 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
         rot_accel += t.rotational * output;
     }
 
-    // Limit movement at the surface of the water
-    if (position.z < 0 && body_accel.z < 0) {
-    	body_accel.z = GRAVITY_MSS;
-    }
-
     // Limit movement at the sea floor
     if (position.z > 100 && body_accel.z > -GRAVITY_MSS) {
     	body_accel.z = -GRAVITY_MSS;
@@ -91,6 +86,29 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
     }
 }
 
+/**
+* @brief Calculate buoyancy force of the frame
+*
+* @return float
+*/
+float Submarine::calculate_buoyancy_acceleration()
+{
+    float below_water_level = position.z - frame_proprietary.height/2;
+
+    // Completely above water level
+    if (below_water_level < 0) {
+        return 0.0f;
+    }
+
+    // Completely below water level
+    if (below_water_level > frame_proprietary.height/2) {
+        return frame_proprietary.bouyancy_acceleration;
+    }
+
+    // bouyant force is proportional to fraction of height in water
+    return frame_proprietary.bouyancy_acceleration * below_water_level/frame_proprietary.height;
+};
+
 /*
   update the Submarine simulation by one time step
  */
diff --git a/libraries/SITL/SIM_Submarine.h b/libraries/SITL/SIM_Submarine.h
index f470076be0..85a8d22768 100644
--- a/libraries/SITL/SIM_Submarine.h
+++ b/libraries/SITL/SIM_Submarine.h
@@ -43,11 +43,25 @@ public:
 
 
 protected:
+    const float water_density = 1023.6; // (kg/m^3) At a temperature of 25 °C, salinity of 35 g/kg and 1 atm pressure
+
+    const struct {
+        float length = 0.457; // x direction (meters)
+        float width  = 0.338; // y direction (meters)
+        float height = 0.254; // z direction (meters)
+        float weight = 10.5;  // (kg)
+        float net_bouyancy = 2.0; // (N)
+
+        float bouyancy_acceleration = GRAVITY_MSS + net_bouyancy/weight;
+    } frame_proprietary;
 
     bool on_ground() const override;
 
     // calculate rotational and linear accelerations
     void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
+    // calculate buoyancy
+    float calculate_buoyancy_acceleration();
+
     Frame *frame;
 };