aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: move motor related constants to motor object

Browse Source
This commit is contained in:
Iampete1 2022-04-13 13:50:36 +01:00 committed by Andrew Tridgell
parent db14ba46dc
commit 7936f3bb0a
4 changed files with 18 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/SITL/SIM_Frame.cpp
View File

@ -465,9 +465,9 @@ void Frame::init(const char *frame_str, Battery *_battery)
float hover_power = model.refCurrent * model.refVoltage; float hover_power = model.refCurrent * model.refVoltage;
float hover_velocity_out = 2 * hover_power / hover_thrust; float hover_velocity_out = 2 * hover_power / hover_thrust;
float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out)); float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out));
velocity_max = hover_velocity_out / sqrtf(model.hoverThrOut); float velocity_max = hover_velocity_out / sqrtf(model.hoverThrOut);
thrust_max = 0.5 * ref_air_density * effective_disc_area * sq(velocity_max); thrust_max = 0.5 * ref_air_density * effective_disc_area * sq(velocity_max);
effective_prop_area = effective_disc_area / num_motors; float effective_prop_area = effective_disc_area / num_motors;
// power_factor is ratio of power consumed per newton of thrust // power_factor is ratio of power consumed per newton of thrust
float power_factor = hover_power / hover_thrust; float power_factor = hover_power / hover_thrust;
@ -476,7 +476,7 @@ void Frame::init(const char *frame_str, Battery *_battery)
for (uint8_t i=0; i<num_motors; i++) { 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, motors[i].setup_params(model.pwmMin, model.pwmMax, model.spin_min, model.spin_max, model.propExpo, model.slew_max,
model.mass, model.diagonal_size, power_factor, model.maxVoltage); model.mass, model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max);
} }
@ -491,7 +491,7 @@ void Frame::init(const char *frame_str, Battery *_battery)
Vector3f rot_accel {}, thrust {}; Vector3f rot_accel {}, thrust {};
Vector3f vel_air_bf {}; Vector3f vel_air_bf {};
motors[0].calculate_forces(input, motor_offset, rot_accel, thrust, vel_air_bf, motors[0].calculate_forces(input, motor_offset, rot_accel, thrust, vel_air_bf,
ref_air_density, velocity_max, effective_prop_area, battery->get_voltage()); ref_air_density, battery->get_voltage());
::printf("pwm[%u] cmd=%.3f thrust=%.3f hovthst=%.3f\n", ::printf("pwm[%u] cmd=%.3f thrust=%.3f hovthst=%.3f\n",
pwm, motors[0].pwm_to_command(pwm), -thrust.z*num_motors, hover_thrust); pwm, motors[0].pwm_to_command(pwm), -thrust.z*num_motors, hover_thrust);
} }
@ -538,8 +538,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
float current = 0; float current = 0;
for (uint8_t i=0; i<num_motors; i++) { for (uint8_t i=0; i<num_motors; i++) {
Vector3f mraccel, mthrust; Vector3f mraccel, mthrust;
motors[i].calculate_forces(input, motor_offset, mraccel, mthrust, vel_air_bf, air_density, velocity_max, motors[i].calculate_forces(input, motor_offset, mraccel, mthrust, vel_air_bf, air_density, battery->get_voltage());
effective_prop_area, battery->get_voltage());
current += motors[i].get_current(); current += motors[i].get_current();
rot_accel += mraccel; rot_accel += mraccel;
thrust += mthrust; thrust += mthrust;

2
libraries/SITL/SIM_Frame.h
View File

@ -134,9 +134,7 @@ private:
// exposed area times coefficient of drag // exposed area times coefficient of drag
float areaCd; float areaCd;
float mass; float mass;
float velocity_max;
float thrust_max; float thrust_max;
float effective_prop_area;
Battery *battery; Battery *battery;
float last_param_voltage; float last_param_voltage;

15
libraries/SITL/SIM_Motor.cpp
View File

@ -28,8 +28,6 @@ void Motor::calculate_forces(const struct sitl_input &input,
Vector3f &thrust, Vector3f &thrust,
const Vector3f &velocity_air_bf, const Vector3f &velocity_air_bf,
float air_density, float air_density,
float velocity_max,
float effective_prop_area,
float voltage) float voltage)
{ {
// fudge factors // fudge factors
@ -64,7 +62,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
float velocity_in = MAX(0, -velocity_air_bf.z); float velocity_in = MAX(0, -velocity_air_bf.z);
// get thrust for untilted motor // get thrust for untilted motor
float motor_thrust = calc_thrust(command, air_density, effective_prop_area, velocity_in, velocity_max * voltage_scale); float motor_thrust = calc_thrust(command, air_density, velocity_in, voltage_scale);
// thrust in NED // thrust in NED
thrust = {0, 0, -motor_thrust}; thrust = {0, 0, -motor_thrust};
@ -153,7 +151,8 @@ float Motor::get_current(void) const
// setup PWM ranges for this motor // 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, void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max) float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
float _velocity_max)
{ {
mot_pwm_min = _pwm_min; mot_pwm_min = _pwm_min;
mot_pwm_max = _pwm_max; mot_pwm_max = _pwm_max;
@ -165,6 +164,8 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min,
diagonal_size = _diagonal_size; diagonal_size = _diagonal_size;
power_factor = _power_factor; power_factor = _power_factor;
voltage_max = _voltage_max; voltage_max = _voltage_max;
effective_prop_area = _effective_prop_area;
max_outflow_velocity = _velocity_max;
// assume 50% of mass on ring around center // assume 50% of mass on ring around center
moment_of_inertia.x = vehicle_mass * 0.25 * sq(diagonal_size*0.5); moment_of_inertia.x = vehicle_mass * 0.25 * sq(diagonal_size*0.5);
@ -186,14 +187,14 @@ float Motor::pwm_to_command(float pwm) const
/* /*
calculate thrust given a command value calculate thrust given a command value
*/ */
float Motor::calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const float Motor::calc_thrust(float command, float air_density, float velocity_in, float voltage_scale) const
{ {
float velocity_out = velocity_max * sqrtf((1-mot_expo)*command + mot_expo*sq(command)); float velocity_out = voltage_scale * max_outflow_velocity * sqrtf((1-mot_expo)*command + mot_expo*sq(command));
float ret = 0.5 * air_density * effective_prop_area * (sq(velocity_out) - sq(velocity_in)); float ret = 0.5 * air_density * effective_prop_area * (sq(velocity_out) - sq(velocity_in));
#if 0 #if 0
if (command > 0) { if (command > 0) {
::printf("air_density=%f effective_prop_area=%f velocity_in=%f velocity_max=%f\n", ::printf("air_density=%f effective_prop_area=%f velocity_in=%f velocity_max=%f\n",
air_density, effective_prop_area, velocity_in, velocity_max); air_density, effective_prop_area, velocity_in, voltage_scale * max_outflow_velocity);
::printf("calc_thrust %.3f %.3f\n", command, ret); ::printf("calc_thrust %.3f %.3f\n", command, ret);
} }
#endif #endif

9
libraries/SITL/SIM_Motor.h
View File

@ -75,8 +75,6 @@ public:
Vector3f &body_thrust, // Z is down Vector3f &body_thrust, // Z is down
const Vector3f &velocity_air_bf, const Vector3f &velocity_air_bf,
float air_density, float air_density,
float velocity_max,
float effective_prop_area,
float voltage); float voltage);
uint16_t update_servo(uint16_t demand, uint64_t time_usec, float &last_value) const; uint16_t update_servo(uint16_t demand, uint64_t time_usec, float &last_value) const;
@ -89,7 +87,8 @@ public:
// setup motor key parameters // 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, void setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max); float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
float _velocity_max);
// override slew limit // override slew limit
void set_slew_max(float _slew_max) { void set_slew_max(float _slew_max) {
@ -101,7 +100,7 @@ public:
} }
// calculate thrust of motor // calculate thrust of motor
float calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const; float calc_thrust(float command, float air_density, float velocity_in, float voltage_scale) const;
private: private:
float mot_pwm_min; float mot_pwm_min;
@ -116,6 +115,8 @@ private:
float power_factor; float power_factor;
float voltage_max; float voltage_max;
Vector3f moment_of_inertia; Vector3f moment_of_inertia;
float effective_prop_area;
float max_outflow_velocity;
float last_command; float last_command;
uint64_t last_calc_us; uint64_t last_calc_us;