SITL: Make Sub buoyancy a parameter

libraries/SITL/SIM_Submarine.cpp

@@ -215,11 +215,11 @@ float Submarine::calculate_buoyancy_acceleration()
 
     // Completely below water level
     if (below_water_level > frame_property.height/2) {
-        return frame_property.buoyancy_acceleration;
+        return GRAVITY_MSS + sitl->buoyancy / frame_property.mass;
     }
 
     // bouyant force is proportional to fraction of height in water
-    return frame_property.buoyancy_acceleration * below_water_level/frame_property.height;
+    return GRAVITY_MSS + (sitl->buoyancy * below_water_level/frame_property.height) / frame_property.mass;
 };
 
 /*

libraries/SITL/SIM_Submarine.h

@@ -65,28 +65,19 @@ protected:
         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;
-
+        float equivalent_sphere_radius = 0.2;
+        // volume = 4.pi.r³/3
+        float volume = 4 * M_PI * pow(equivalent_sphere_radius, 3) / 3;
+        float density = 500;
+        float mass = volume * density; // 16.75 kg
         // 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;
+        float moment_of_inertia =  2 * (mass * pow(equivalent_sphere_radius, 2) / 5);
 
         // Frame drag coefficient
-        const Vector3f linear_drag_coefficient = Vector3f(0.2, 0.3, 0.4);
+        const Vector3f linear_drag_coefficient = Vector3f(1.4, 1.8, 2.0);
         // Angular drag coefficient CD for a cube is 1.05. This is subject to change based on experimentation.
         const Vector3f angular_drag_coefficient = Vector3f(1.05, 1.05, 1.05);
-        // 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 $

libraries/SITL/SITL.cpp

@@ -238,6 +238,9 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     // vicon sensor position (position offsets in body frame)
     AP_GROUPINFO("VICON_POS",     14, SITL,  vicon_pos_offset, 0),
 
+    // Buyoancy for submarines
+    AP_GROUPINFO_FRAME("BUOYANCY", 15, SITL, buoyancy, 1, AP_PARAM_FRAME_SUB),
+
     AP_GROUPEND
 
 };

libraries/SITL/SITL.h

@@ -191,6 +191,7 @@ public:
     AP_Int32 loop_delay; // extra delay to add to every loop
     AP_Float mag_scaling; // scaling factor on first compasses
     AP_Int32 mag_devid[MAX_CONNECTED_MAGS]; // Mag devid
+    AP_Float buoyancy; // submarine buoyancy in Newtons
 
     // EFI type
     enum EFIType {