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

SITL: apply momentum drag per-motor

Browse Source
This commit is contained in:
Iampete1 2022-04-23 00:30:47 +01:00 committed by Peter Barker
parent f9050c3040
commit 6eea4d12ee
3 changed files with 34 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/SITL/SIM_Frame.cpp
View File

@ -516,6 +516,7 @@ void Frame::init(const char *frame_str, Battery *_battery)
float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out));
float velocity_max = hover_velocity_out / sqrtf(model.hoverThrOut);
float effective_prop_area = effective_disc_area / num_motors;
float true_prop_area = model.disc_area / num_motors;
// power_factor is ratio of power consumed per newton of thrust
float power_factor = hover_power / hover_thrust;
@ -525,7 +526,8 @@ void Frame::init(const char *frame_str, Battery *_battery)
for (uint8_t i=0; i<num_motors; i++) {
motors[i].setup_params(model.pwmMin, model.pwmMax, model.spin_min, model.spin_max, model.propExpo, model.slew_max,
model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max,
model.motor_pos[i], model.motor_thrust_vec[i], model.yaw_factor[i]);
model.motor_pos[i], model.motor_thrust_vec[i], model.yaw_factor[i], true_prop_area,
model.mdrag_coef);
}
if (is_zero(model.moment_of_inertia.x) || is_zero(model.moment_of_inertia.y) || is_zero(model.moment_of_inertia.z)) {
@ -596,7 +598,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
float current = 0;
for (uint8_t i=0; i<num_motors; i++) {
Vector3f mtorque, mthrust;
motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, gyro, air_density, battery->get_voltage());
motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, gyro, air_density, battery->get_voltage(), use_drag);
current += motors[i].get_current();
torque += mtorque;
thrust += mthrust;
@ -621,24 +623,17 @@ void Frame::calculate_forces(const Aircraft &aircraft,
if (use_drag) {
// use the model params to calculate drag
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);
drag_bf.x = areaCd * 0.5f * air_density * sq(vel_air_bf.x);
if (is_negative(vel_air_bf.x)) {
drag_bf.x = -drag_bf.x;
}
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);
drag_bf.y = areaCd * 0.5f * air_density * sq(vel_air_bf.y);
if (is_negative(vel_air_bf.y)) {
drag_bf.y = -drag_bf.y;
}
// The application of momentum drag to the Z axis is a 'hack' to compensate for incorrect modelling
// of the variation of thust with vel_air_bf.z in SIM_Motor.cpp. If nmot applied, the vehicle will
// climb at an unrealistic rate during operation in STABILIZE. TODO replace prop and motor model in
// the Motor class with one based on DC motor, mometum disc and blade elemnt theory.
drag_bf.z = areaCd * 0.5f * air_density * sq(vel_air_bf.z) +
model.mdrag_coef * fabsf(vel_air_bf.z) * sqrtf(fabsf(thrust.z) * air_density * model.disc_area);
drag_bf.z = areaCd * 0.5f * air_density * sq(vel_air_bf.z);
if (is_negative(vel_air_bf.z)) {
drag_bf.z = -drag_bf.z;
}

23
libraries/SITL/SIM_Motor.cpp
View File

@ -29,7 +29,8 @@ void Motor::calculate_forces(const struct sitl_input &input,
const Vector3f &velocity_air_bf,
const Vector3f &gyro,
float air_density,
float voltage)
float voltage,
bool use_drag)
{
// fudge factors
const float yaw_scale = radians(40);
@ -109,6 +110,21 @@ void Motor::calculate_forces(const struct sitl_input &input,
torque = rotation * torque;
}
if (use_drag) {
// calculate momentum drag per motor
const float momentum_drag_factor = momentum_drag_coefficient * sqrtf(air_density * true_prop_area);
Vector3f momentum_drag;
momentum_drag.x = momentum_drag_factor * motor_vel.x * (sqrtf(fabsf(thrust.y)) + sqrtf(fabsf(thrust.z)));
momentum_drag.y = momentum_drag_factor * motor_vel.y * (sqrtf(fabsf(thrust.x)) + sqrtf(fabsf(thrust.z)));
// The application of momentum drag to the Z axis is a 'hack' to compensate for incorrect modelling
// of the variation of thust with inflow velocity. If not applied, the vehicle will
// climb at an unrealistic rate during operation in STABILIZE. TODO replace prop and motor model in
// with one based on DC motor, momentum disc and blade element theory.
momentum_drag.z = momentum_drag_factor * motor_vel.z * (sqrtf(fabsf(thrust.x)) + sqrtf(fabsf(thrust.y)) + sqrtf(fabsf(thrust.z)));
thrust -= momentum_drag;
}
// calculate current
float power = power_factor * fabsf(motor_thrust);
current = power / MAX(voltage, 0.1);
@ -150,7 +166,8 @@ float Motor::get_current(void) const
// setup PWM ranges for this motor
void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
float _velocity_max, Vector3f _position, Vector3f _thrust_vector, float _yaw_factor)
float _velocity_max, Vector3f _position, Vector3f _thrust_vector, float _yaw_factor,
float _true_prop_area, float _momentum_drag_coefficient)
{
mot_pwm_min = _pwm_min;
mot_pwm_max = _pwm_max;
@ -162,6 +179,8 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min,
voltage_max = _voltage_max;
effective_prop_area = _effective_prop_area;
max_outflow_velocity = _velocity_max;
true_prop_area = _true_prop_area;
momentum_drag_coefficient = _momentum_drag_coefficient;
if (!_position.is_zero()) {
position = _position;

8
libraries/SITL/SIM_Motor.h
View File

@ -93,7 +93,8 @@ public:
const Vector3f &velocity_air_bf,
const Vector3f &gyro, // rad/sec
float air_density,
float voltage);
float voltage,
bool use_drag);
uint16_t update_servo(uint16_t demand, uint64_t time_usec, float &last_value) const;
@ -106,7 +107,8 @@ public:
// setup motor key parameters
void setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
float _velocity_max, Vector3f _position, Vector3f _thrust_vector, float _yaw_factor);
float _velocity_max, Vector3f _position, Vector3f _thrust_vector, float _yaw_factor,
float _true_prop_area, float _momentum_drag_coefficient);
// override slew limit
void set_slew_max(float _slew_max) {
@ -132,6 +134,8 @@ private:
float voltage_max;
float effective_prop_area;
float max_outflow_velocity;
float true_prop_area;
float momentum_drag_coefficient;
float last_command;
uint64_t last_calc_us;