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

SITL: motor: use postion and thrust vector

Browse Source
This commit is contained in:
Iampete1 2022-04-13 20:58:16 +01:00 committed by Andrew Tridgell
parent 47f327b500
commit c4518e5cb7
2 changed files with 29 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/SITL/SIM_Motor.cpp
View File

@ -56,20 +56,16 @@ void Motor::calculate_forces(const struct sitl_input &input,
last_command = command; last_command = command;
// the yaw torque of the motor // the yaw torque of the motor
Vector3f rotor_torque(0, 0, yaw_factor * command * yaw_scale * voltage_scale); Vector3f rotor_torque = thrust_vector * yaw_factor * command * yaw_scale * voltage_scale * -1.0;
// calculate velocity into prop, clipping at zero, assumes zero roll/pitch // calculate velocity into prop, clipping at zero, assumes zero roll/pitch
float velocity_in = MAX(0, -velocity_air_bf.z); float velocity_in = MAX(0, -velocity_air_bf.projected(thrust_vector).z);
// get thrust for untilted motor // get thrust for untilted motor
float motor_thrust = calc_thrust(command, air_density, velocity_in, 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 = thrust_vector * motor_thrust;
// define the arm position relative to center of mass
Vector3f arm(cosf(radians(angle)), sinf(radians(angle)), 0);
arm *= diagonal_size;
// work out roll and pitch of motor relative to it pointing straight up // work out roll and pitch of motor relative to it pointing straight up
float roll = 0, pitch = 0; float roll = 0, pitch = 0;
@ -96,7 +92,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
last_change_usec = now; last_change_usec = now;
// calculate torque in newton-meters // calculate torque in newton-meters
torque = (arm % thrust) + rotor_torque; torque = (position % thrust) + rotor_torque;
// possibly rotate the thrust vector and the rotor torque // possibly rotate the thrust vector and the rotor torque
if (!is_zero(roll) || !is_zero(pitch)) { if (!is_zero(roll) || !is_zero(pitch)) {
@ -155,11 +151,14 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min,
mot_spin_max = _spin_max; mot_spin_max = _spin_max;
mot_expo = _expo; mot_expo = _expo;
slew_max = _slew_max; slew_max = _slew_max;
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; effective_prop_area = _effective_prop_area;
max_outflow_velocity = _velocity_max; max_outflow_velocity = _velocity_max;
position.x = cosf(radians(angle)) * _diagonal_size;
position.y = sinf(radians(angle)) * _diagonal_size;
position.z = 0;
} }
/* /*

24
libraries/SITL/SIM_Motor.h
View File

@ -49,7 +49,15 @@ public:
angle(_angle), // angle in degrees from front angle(_angle), // angle in degrees from front
yaw_factor(_yaw_factor), // positive is clockwise yaw_factor(_yaw_factor), // positive is clockwise
display_order(_display_order) // order for clockwise display display_order(_display_order) // order for clockwise display
{} {
position.x = cosf(radians(angle));
position.y = sinf(radians(angle));
position.z = 0;
thrust_vector.x = 0;
thrust_vector.y = 0;
thrust_vector.z = -1;
}
/* /*
alternative constructor for tiltable motors alternative constructor for tiltable motors
@ -67,7 +75,15 @@ public:
pitch_servo(_pitch_servo), pitch_servo(_pitch_servo),
pitch_min(_pitch_min), pitch_min(_pitch_min),
pitch_max(_pitch_max) pitch_max(_pitch_max)
{} {
position.x = cosf(radians(angle));
position.y = sinf(radians(angle));
position.z = 0;
thrust_vector.x = 0;
thrust_vector.y = 0;
thrust_vector.z = -1;
}
void calculate_forces(const struct sitl_input &input, void calculate_forces(const struct sitl_input &input,
uint8_t motor_offset, uint8_t motor_offset,
@ -109,7 +125,6 @@ private:
float mot_spin_max; float mot_spin_max;
float mot_expo; float mot_expo;
float slew_max; float slew_max;
float diagonal_size;
float current; float current;
float power_factor; float power_factor;
float voltage_max; float voltage_max;
@ -118,6 +133,9 @@ private:
float last_command; float last_command;
uint64_t last_calc_us; uint64_t last_calc_us;
Vector3f position;
Vector3f thrust_vector;
}; };
} }