Sub: Add fake sea floor and update range

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>

libraries/SITL/SIM_Submarine.cpp

@@ -87,8 +87,10 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
         rot_accel += t.rotational * output * frame_property.thrust * frame_property.thruster_mount_radius / frame_property.moment_of_inertia;
     }
 
+    float floor_depth = calculate_sea_floor_depth(position);
+    range = floor_depth - position.z;
     // Limit movement at the sea floor
-    if (position.z > 100 && body_accel.z > -GRAVITY_MSS) {
+    if (position.z > floor_depth && body_accel.z > -GRAVITY_MSS) {
     	body_accel.z = -GRAVITY_MSS;
     }
 
@@ -127,6 +129,19 @@ void Submarine::calculate_buoyancy_torque(Vector3f &torque)
 }
 
 
+/**
+ * @brief Calculate sea floor depth from submarine position
+ *          This creates a non planar floor for rangefinder sensor test
+ *          TODO: Create a better sea floor with procedural generatation
+ *
+ * @param position
+ * @return float
+ */
+float Submarine::calculate_sea_floor_depth(const Vector3f &/*position*/)
+{
+    return 50;
+}
+
 /**
  * @brief Calculate drag force against body
  *

libraries/SITL/SIM_Submarine.h

@@ -98,6 +98,8 @@ protected:
 
     bool on_ground() const override;
 
+    // calculate sea floor depth based for terrain follow
+    float calculate_sea_floor_depth(const Vector3f &/*position*/);
     // calculate rotational and linear accelerations
     void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
     // calculate buoyancy