From e52fc375c498932b5949527217e7cdc2924d0b74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Tue, 29 Oct 2019 09:07:10 -0700 Subject: [PATCH] Sub: Add fake sea floor and update range MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- libraries/SITL/SIM_Submarine.cpp | 17 ++++++++++++++++- libraries/SITL/SIM_Submarine.h | 2 ++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp index bad8895f48..7c84f3084e 100644 --- a/libraries/SITL/SIM_Submarine.cpp +++ b/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 * diff --git a/libraries/SITL/SIM_Submarine.h b/libraries/SITL/SIM_Submarine.h index de2781f259..36ab9079c1 100644 --- a/libraries/SITL/SIM_Submarine.h +++ b/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