Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Add fake sea floor and update range

Browse Source 
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
Patrick José Pereira 2019-10-29 09:07:10 -07:00 committed by Jacob Walser
parent 762b2c9541
commit e52fc375c4
2 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/SITL/SIM_Submarine.cpp
View File

@ -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
*

2
libraries/SITL/SIM_Submarine.h
View File

@ -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