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

SITL: move moments inertia to frame property

Browse Source
This commit is contained in:
Iampete1 2022-04-13 15:03:53 +01:00 committed by Andrew Tridgell
parent 7936f3bb0a
commit 6272dc33dd
4 changed files with 23 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/SITL/SIM_Frame.cpp
View File

@ -476,9 +476,14 @@ 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.mass, model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max);
model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max);
}
// assume 50% of mass on ring around center
model.moment_of_inertia.x = model.mass * 0.25 * sq(model.diagonal_size*0.5);
model.moment_of_inertia.y = model.moment_of_inertia.x;
model.moment_of_inertia.z = model.mass * 0.5 * sq(model.diagonal_size*0.5);
#if 0
// useful debug code for thrust curve
@ -530,6 +535,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
bool use_drag)
{
Vector3f thrust; // newtons
Vector3f torque;
const float air_density = get_air_density(aircraft.get_location().alt*0.01);
@ -537,10 +543,10 @@ void Frame::calculate_forces(const Aircraft &aircraft,
float current = 0;
for (uint8_t i=0; i<num_motors; i++) {
Vector3f mraccel, mthrust;
motors[i].calculate_forces(input, motor_offset, mraccel, mthrust, vel_air_bf, air_density, battery->get_voltage());
Vector3f mtorque, mthrust;
motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, air_density, battery->get_voltage());
current += motors[i].get_current();
rot_accel += mraccel;
torque += mtorque;
thrust += mthrust;
// simulate motor rpm
if (!is_zero(AP::sitl()->vibe_motor)) {
@ -548,6 +554,11 @@ void Frame::calculate_forces(const Aircraft &aircraft,
}
}
// calculate total rotational acceleration
rot_accel.x = torque.x / model.moment_of_inertia.x;
rot_accel.y = torque.y / model.moment_of_inertia.y;
rot_accel.z = torque.z / model.moment_of_inertia.z;
body_accel = thrust/aircraft.gross_mass();
if (terminal_rotation_rate > 0) {

3
libraries/SITL/SIM_Frame.h
View File

@ -127,6 +127,9 @@ private:
// momentum drag coefficient
float mdrag_coef = 0.2;
// if zero value will be estimated from mass
Vector3f moment_of_inertia;
} default_model;
struct Model model;

19
libraries/SITL/SIM_Motor.cpp
View File

@ -24,7 +24,7 @@ using namespace SITL;
// calculate rotational accel and thrust for a motor
void Motor::calculate_forces(const struct sitl_input &input,
uint8_t motor_offset,
Vector3f &rot_accel,
Vector3f &torque,
Vector3f &thrust,
const Vector3f &velocity_air_bf,
float air_density,
@ -39,7 +39,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
if (voltage_scale < 0.1) {
// battery is dead
rot_accel.zero();
torque.zero();
thrust.zero();
current = 0;
return;
@ -96,7 +96,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
last_change_usec = now;
// calculate torque in newton-meters
Vector3f torque = (arm % thrust) + rotor_torque;
torque = (arm % thrust) + rotor_torque;
// possibly rotate the thrust vector and the rotor torque
if (!is_zero(roll) || !is_zero(pitch)) {
@ -106,11 +106,6 @@ void Motor::calculate_forces(const struct sitl_input &input,
torque = rotation * torque;
}
// calculate total rotational acceleration
rot_accel.x = torque.x / moment_of_inertia.x;
rot_accel.y = torque.y / moment_of_inertia.y;
rot_accel.z = torque.z / moment_of_inertia.z;
// calculate current
float power = power_factor * fabsf(motor_thrust);
current = power / MAX(voltage, 0.1);
@ -151,7 +146,7 @@ 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 _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
float _velocity_max)
{
mot_pwm_min = _pwm_min;
@ -160,17 +155,11 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min,
mot_spin_max = _spin_max;
mot_expo = _expo;
slew_max = _slew_max;
vehicle_mass = _vehicle_mass;
diagonal_size = _diagonal_size;
power_factor = _power_factor;
voltage_max = _voltage_max;
effective_prop_area = _effective_prop_area;
max_outflow_velocity = _velocity_max;
// assume 50% of mass on ring around center
moment_of_inertia.x = vehicle_mass * 0.25 * sq(diagonal_size*0.5);
moment_of_inertia.y = moment_of_inertia.x;
moment_of_inertia.z = vehicle_mass * 0.5 * sq(diagonal_size*0.5);
}
/*

4
libraries/SITL/SIM_Motor.h
View File

@ -87,7 +87,7 @@ 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 _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
float _velocity_max);
// override slew limit
@ -109,12 +109,10 @@ private:
float mot_spin_max;
float mot_expo;
float slew_max;
float vehicle_mass;
float diagonal_size;
float current;
float power_factor;
float voltage_max;
Vector3f moment_of_inertia;
float effective_prop_area;
float max_outflow_velocity;