SITL: Submarine: add torque for static stability

Subs are statically stable regarding pitch and roll.
This patch reproduces this in SITL

libraries/SITL/SIM_Submarine.cpp

@@ -100,12 +100,33 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
     body_accel -= linear_drag_forces / frame_property.weight;
 
     // 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);
+
+    // 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 += buoyancy_torque / frame_property.moment_of_inertia;
 }
 
+
+/**
+ * @brief Calculate the torque induced by buoyancy foam
+ *
+ * @param torque Output torques
+ */
+void Submarine::calculate_buoyancy_torque(Vector3f &torque)
+{
+    // Let's assume 2 Liters water displacement at the top, and ~ 2kg of weight at the bottom.
+    const Vector3f force_up(0,0,-40); // 40 N upwards
+    const Vector3f force_position = dcm.transposed() * Vector3f(0, 0, 0.15); // offset in meters
+    torque = force_position % force_up;
+}
+
+
 /**
  * @brief Calculate drag force against body
  *

libraries/SITL/SIM_Submarine.h

@@ -63,6 +63,18 @@ protected:
         float width  = 0.338; // y direction (meters)
         float height = 0.254; // z direction (meters)
         float weight = 10.5;  // (kg)
+        float thrust = 51.48; // (N)
+        float thruster_mount_radius = 0.25; // distance in meters from thrusters to center of mass. Used to calculate torque.
+        float equivalent_sphere_radius = 0.25;
+
+        // Moment of Inertia (I)(kg.m²) approximated with a sphere with a 25 cm radius (r) and same density as water
+        // I = 2.m.r²/5
+        // mass = volume* density
+        // volume = 4.pi.r³/3
+        //                             ,-----------------------------------Mass--------------------.
+        //                             ||------------------------Volume-----------------| |density||
+        float moment_of_inertia =  2 * (4 * M_PI * pow(equivalent_sphere_radius, 3) / 3)  *  1000 * pow(equivalent_sphere_radius, 2) / 5;
+
         float net_buoyancy = 2.0; // (N)
 
         float buoyancy_acceleration = GRAVITY_MSS + net_buoyancy/weight;
@@ -91,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 induced by buoyancy foams
+    void calculate_buoyancy_torque(Vector3f &torque);
 
     Frame *frame;
     Thruster* thrusters;