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

SITL: Sub: Fix rotational physics logic

This commit is contained in:
Willian Galvani 2019-09-18 14:15:23 -03:00 committed by Jacob Walser
parent fbbfbe0103
commit 393b018f31
2 changed files with 21 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/SITL/SIM_Submarine.cpp
View File

@ -100,14 +100,14 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
// Calculate angular drag forces
// TODO: This results in the wrong units. Fix the math.
Vector3f angular_drag_forces;
calculate_drag_force(gyro, frame_property.angular_drag_coefficient, angular_drag_forces);
Vector3f angular_drag_torque;
calculate_angular_drag_torque(gyro, frame_property.angular_drag_coefficient, angular_drag_torque);
// Calculate torque induced by buoyancy foams on the frame
Vector3f buoyancy_torque;
calculate_buoyancy_torque(buoyancy_torque);
// Add forces in body frame accel
rot_accel -= angular_drag_forces / frame_property.weight;
rot_accel -= angular_drag_torque / frame_property.moment_of_inertia;
rot_accel += buoyancy_torque / frame_property.moment_of_inertia;
add_shove_forces(rot_accel, body_accel);
}
@ -153,6 +153,21 @@ void Submarine::calculate_drag_force(const Vector3f &velocity, const Vector3f &d
force *= drag_coefficient;
}
/**
* @brief Calculate angular drag torque
*
* @param angular_velocity Body frame velocity of fluid
* @param drag_coefficient Rotational drag coefficient of body
*/
void Submarine::calculate_angular_drag_torque(const Vector3f &angular_velocity, const Vector3f &drag_coefficient, Vector3f &torque)
{
// TODO: Find a good approximation for this.
// While this works, it is not accurate nor has the right dimensions.
torque = angular_velocity;
torque *= drag_coefficient;
}
/**
* @brief Calculate buoyancy force of the frame
*

4
libraries/SITL/SIM_Submarine.h
View File

@ -81,7 +81,7 @@ protected:
// Frame drag coefficient
const Vector3f linear_drag_coefficient = Vector3f(0.2, 0.3, 0.4);
const Vector3f angular_drag_coefficient = Vector3f(1, 1, 1);
const Vector3f angular_drag_coefficient = Vector3f(5, 5, 5);
// 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)
@ -103,6 +103,8 @@ protected:
float calculate_buoyancy_acceleration();
// calculate drag from velocity and drag coefficient
void calculate_drag_force(const Vector3f &velocity, const Vector3f &drag_coefficient, Vector3f &force);
// calculate torque water resistance
void calculate_angular_drag_torque(const Vector3f &angular_velocity, const Vector3f &drag_coefficient, Vector3f &torque);
// calculate torque induced by buoyancy foams
void calculate_buoyancy_torque(Vector3f &torque);