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

SITL: Add momentum drag to multicopter model

Browse Source
This commit is contained in:
Paul Riseborough 2020-11-29 09:51:58 +11:00 committed by Andrew Tridgell
parent abd3ae8f3b
commit a907c10733
2 changed files with 41 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/SITL/SIM_Frame.cpp
View File

@ -395,6 +395,8 @@ void Frame::load_frame_params(const char *model_json)
FRAME_VAR(spin_min),
FRAME_VAR(spin_max),
FRAME_VAR(slew_max),
FRAME_VAR(disc_area),
FRAME_VAR(mdrag_coef),
};
static_assert(sizeof(model) == sizeof(float)*ARRAY_SIZE(vars), "incorrect model vars");
@ -430,9 +432,21 @@ void Frame::init(const char *frame_str, Battery *_battery)
const float drag_force = model.mass * GRAVITY_MSS * tanf(radians(model.refAngle));
const float cos_tilt = cosf(radians(model.refAngle));
const float airspeed_bf = model.refSpd * cos_tilt;
const float ref_thrust = model.mass * GRAVITY_MSS / cos_tilt;
float ref_air_density = get_air_density(model.refAlt);
areaCd = drag_force / (0.5 * ref_air_density * sq(model.refSpd));
const float momentum_drag = cos_tilt * model.mdrag_coef * airspeed_bf * sqrtf(ref_thrust * ref_air_density * model.disc_area);
if (momentum_drag > drag_force) {
model.mdrag_coef *= drag_force / momentum_drag;
areaCd = 0.0;
::printf("Suggested EK3_BCOEF_* = 0, EK3_MCOEF = %.3f\n", (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density));
} else {
areaCd = (drag_force - momentum_drag) / (0.5f * ref_air_density * sq(model.refSpd));
::printf("Suggested EK3_BCOEF_* = %.3f, EK3_MCOEF = %.3f\n", model.mass / areaCd, (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density));
}
terminal_rotation_rate = model.refRotRate;
@ -534,14 +548,25 @@ void Frame::calculate_forces(const Aircraft &aircraft,
if (use_drag) {
// use the model params to calculate drag
const Vector3f vel_air_ef = aircraft.get_velocity_air_ef();
const float speed = vel_air_ef.length();
float drag_force = areaCd * 0.5 * air_density * sq(speed);
Vector3f drag_ef;
if (is_positive(speed)) {
drag_ef = -(vel_air_ef / speed) * drag_force;
Vector3f drag_bf;
drag_bf.x = areaCd * 0.5f * air_density * sq(vel_air_bf.x) +
model.mdrag_coef * fabsf(vel_air_bf.x) * sqrtf(fabsf(thrust.z) * air_density * model.disc_area);
if (is_positive(vel_air_bf.x)) {
drag_bf.x = -drag_bf.x;
}
body_accel += aircraft.get_dcm().transposed() * drag_ef / mass;
drag_bf.y = areaCd * 0.5f * air_density * sq(vel_air_bf.y) +
model.mdrag_coef * fabsf(vel_air_bf.y) * sqrtf(fabsf(thrust.z) * air_density * model.disc_area);
if (is_positive(vel_air_bf.y)) {
drag_bf.y = -drag_bf.y;
}
drag_bf.z = areaCd * 0.5f * air_density * sq(vel_air_bf.z);
if (is_positive(vel_air_bf.z)) {
drag_bf.z = -drag_bf.z;
}
body_accel += drag_bf / mass;
}
// add some noise

8
libraries/SITL/SIM_Frame.h
View File

@ -119,6 +119,14 @@ private:
// maximum slew rate of motors
float slew_max = 150;
// rotor disc area in m**2 for 4 x 0.35m dia rotors
// Note that coaxial rotors count as one rotor only when cauclating effective disc area
float disc_area = 0.385;
// momentum drag coefficient
float mdrag_coef = 0.2;
} default_model;
struct Model model;