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

SITL: Add drag simulation in Sub

Browse Source 
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
Patrick José Pereira 2019-03-04 13:18:28 -08:00 committed by Peter Barker
parent 0e5f9454dd
commit 8bb0772224
2 changed files with 53 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/SITL/SIM_Submarine.cpp
View File

@ -70,20 +70,43 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
body_accel.z = -GRAVITY_MSS;
}
float terminal_rotation_rate = 10.0;
if (terminal_rotation_rate > 0) {
// rotational air resistance
rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
}
// Calculate linear drag forces
Vector3f linear_drag_forces;
calculate_drag_force(velocity_air_bf, frame_property.linear_drag_coefficient, linear_drag_forces);
// Add forces in body frame accel
body_accel -= linear_drag_forces / frame_property.weight;
float terminal_velocity = 3.0;
if (terminal_velocity > 0) {
// air resistance
Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
body_accel += dcm.transposed() * air_resistance;
}
// Calculate angular drag forces
Vector3f angular_drag_forces;
calculate_drag_force(gyro, frame_property.angular_drag_coefficient, angular_drag_forces);
// Add forces in body frame accel
rot_accel -= angular_drag_forces / frame_property.weight;
}
/**
* @brief Calculate drag force against body
*
* @param velocity Body frame velocity of fluid
* @param drag_coefficient Drag coefficient of body
* @param force Output forces
* $ F_D = rho * v^2 * A * C_D / 2 $
* rho = water density (kg/m^3), V = velocity (m/s), A = area (m^2), C_D = drag_coefficient
*/
void Submarine::calculate_drag_force(const Vector3f &velocity, const Vector3f &drag_coefficient, Vector3f &force)
{
/**
* @brief It's necessary to keep the velocity orientation from the body frame.
* To do so, a mathematical artifice is used to do velocity square but without loosing the direction.
* $(|V|/V)*V^2$ = $|V|*V$
*/
const Vector3f velocity_2(
fabsf(velocity.x) * velocity.x,
fabsf(velocity.y) * velocity.y,
fabsf(velocity.z) * velocity.z
);
force = (velocity_2 * water_density) * frame_property.equivalent_sphere_area / 2.0f;
force *= drag_coefficient;
}
/**

17
libraries/SITL/SIM_Submarine.h
View File

@ -53,6 +53,21 @@ protected:
float net_bouyancy = 2.0; // (N)
float bouyancy_acceleration = GRAVITY_MSS + net_bouyancy/weight;
// Frame drag coefficient
const Vector3f linear_drag_coefficient = Vector3f(0.2, 0.3, 0.4);
const Vector3f angular_drag_coefficient = Vector3f(1, 1, 1);
// 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)
float volume = buoyancy_acceleration * weight / (GRAVITY_MSS * 1023.6f);
// Calculate equivalent sphere area for drag force
// $ A = pi * r^2 / 4 $
// $ V = 4 * pi * r^3 / 3 $
// $ r ^2 = (V * 3 / 4) ^ (2/3) $
// A = area (m^3), r = sphere radius (m)
float equivalent_sphere_area = M_PI_4 * pow(volume * 3.0f / 4.0f, 2.0f / 3.0f);
} frame_property;
bool on_ground() const override;
@ -61,6 +76,8 @@ protected:
void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
// calculate buoyancy
float calculate_buoyancy_acceleration();
// calculate drag from velocity and drag coefficient
void calculate_drag_force(const Vector3f &velocity, const Vector3f &drag_coefficient, Vector3f &force);
Frame *frame;
};