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: Motor: acount for airflow due to vehicle rotation

Browse Source
This commit is contained in:
Iampete1 2022-04-20 18:53:11 +01:00 committed by Peter Barker
parent bead0ac0f8
commit f869506b0a
3 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/SITL/SIM_Frame.cpp
View File

@ -590,13 +590,14 @@ void Frame::calculate_forces(const Aircraft &aircraft,
Vector3f torque; Vector3f torque;
const float air_density = get_air_density(aircraft.get_location().alt*0.01); const float air_density = get_air_density(aircraft.get_location().alt*0.01);
const Vector3f gyro = aircraft.get_gyro();
Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef(); Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();
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 mtorque, mthrust; Vector3f mtorque, mthrust;
motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, air_density, battery->get_voltage()); motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, gyro, air_density, battery->get_voltage());
current += motors[i].get_current(); current += motors[i].get_current();
torque += mtorque; torque += mtorque;
thrust += mthrust; thrust += mthrust;
@ -615,7 +616,6 @@ void Frame::calculate_forces(const Aircraft &aircraft,
if (terminal_rotation_rate > 0) { if (terminal_rotation_rate > 0) {
// rotational air resistance // rotational air resistance
const Vector3f &gyro = aircraft.get_gyro();
rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate; rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate; rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate; rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;

13
libraries/SITL/SIM_Motor.cpp
View File

@ -27,6 +27,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
Vector3f &torque, Vector3f &torque,
Vector3f &thrust, Vector3f &thrust,
const Vector3f &velocity_air_bf, const Vector3f &velocity_air_bf,
const Vector3f &gyro,
float air_density, float air_density,
float voltage) float voltage)
{ {
@ -58,13 +59,19 @@ void Motor::calculate_forces(const struct sitl_input &input,
// the yaw torque of the motor // the yaw torque of the motor
Vector3f rotor_torque = thrust_vector * yaw_factor * command * yaw_scale * voltage_scale * -1.0; 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 // velocity of motor through air
float velocity_in = MAX(0, -velocity_air_bf.projected(thrust_vector).z); Vector3f motor_vel = velocity_air_bf;
// add velocity of motor about center due to vehicle rotation
motor_vel += -(position % gyro);
// calculate velocity into prop, clipping at zero
float velocity_in = MAX(0, -motor_vel.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 bodyframe NED
thrust = thrust_vector * motor_thrust; thrust = thrust_vector * motor_thrust;
// 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

5
libraries/SITL/SIM_Motor.h
View File

@ -88,9 +88,10 @@ public:
void calculate_forces(const struct sitl_input &input, void calculate_forces(const struct sitl_input &input,
uint8_t motor_offset, uint8_t motor_offset,
Vector3f &rot_accel, // rad/sec Vector3f &torque, // Newton meters
Vector3f &body_thrust, // Z is down Vector3f &thrust, // Z is down, Newtons
const Vector3f &velocity_air_bf, const Vector3f &velocity_air_bf,
const Vector3f &gyro, // rad/sec
float air_density, float air_density,
float voltage); float voltage);