Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SIM_Submarine: Solve problem when frame is above water level 

Fix bluerobotics/ardusub/issues/128

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
Co-Authored-By: jaxxzer <jwalser90@gmail.com>
This commit is contained in:
Patrick José Pereira 2017-11-22 14:09:43 -02:00 committed by jaxxzer
parent c056076e85
commit 28ff272017
2 changed files with 38 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/SITL/SIM_Submarine.cpp
View File

@ -49,7 +49,7 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
rot_accel = Vector3f(0,0,0);
// slight positive buoyancy
body_accel = Vector3f(0,0,-GRAVITY_MSS * 1.1);
body_accel = Vector3f(0, 0, -calculate_buoyancy_acceleration());
for (int i = 0; i < 6; i++) {
Thruster t = vectored_thrusters[i];
@ -65,11 +65,6 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
rot_accel += t.rotational * output;
}
// Limit movement at the surface of the water
if (position.z < 0 && body_accel.z < 0) {
body_accel.z = GRAVITY_MSS;
}
// Limit movement at the sea floor
if (position.z > 100 && body_accel.z > -GRAVITY_MSS) {
body_accel.z = -GRAVITY_MSS;
@ -91,6 +86,29 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
}
}
/**
* @brief Calculate buoyancy force of the frame
*
* @return float
*/
float Submarine::calculate_buoyancy_acceleration()
{
float below_water_level = position.z - frame_proprietary.height/2;
// Completely above water level
if (below_water_level < 0) {
return 0.0f;
}
// Completely below water level
if (below_water_level > frame_proprietary.height/2) {
return frame_proprietary.bouyancy_acceleration;
}
// bouyant force is proportional to fraction of height in water
return frame_proprietary.bouyancy_acceleration * below_water_level/frame_proprietary.height;
};
/*
update the Submarine simulation by one time step
*/

14
libraries/SITL/SIM_Submarine.h
View File

@ -43,11 +43,25 @@ public:
protected:
const float water_density = 1023.6; // (kg/m^3) At a temperature of 25 °C, salinity of 35 g/kg and 1 atm pressure
const struct {
float length = 0.457; // x direction (meters)
float width = 0.338; // y direction (meters)
float height = 0.254; // z direction (meters)
float weight = 10.5; // (kg)
float net_bouyancy = 2.0; // (N)
float bouyancy_acceleration = GRAVITY_MSS + net_bouyancy/weight;
} frame_proprietary;
bool on_ground() const override;
// calculate rotational and linear accelerations
void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
// calculate buoyancy
float calculate_buoyancy_acceleration();
Frame *frame;
};