Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: update conventional heli dynamics and add blade 360 heli dynamics

This commit is contained in:
Bill Geyer 2021-05-17 23:00:12 -04:00 committed by Peter Barker
parent 35a93c5988
commit 9a917abf76
2 changed files with 115 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
libraries/SITL/SIM_Helicopter.cpp
View File

@ -27,28 +27,40 @@ Helicopter::Helicopter(const char *frame_str) :
{ {
mass = 4.54f; mass = 4.54f;
if (strstr(frame_str, "-dual")) {
frame_type = HELI_FRAME_DUAL;
_time_delay = 30;
nominal_rpm = 1300;
} else if (strstr(frame_str, "-compound")) {
frame_type = HELI_FRAME_COMPOUND;
_time_delay = 50;
nominal_rpm = 1500;
} else if (strstr(frame_str, "-blade360")) {
frame_type = HELI_FRAME_BLADE;
_time_delay = 40;
nominal_rpm = 2100;
} else {
frame_type = HELI_FRAME_CONVENTIONAL;
_time_delay = 50;
nominal_rpm = 1500;
}
/* /*
For conventional and compound
scaling from motor power to Newtons. Allows the copter scaling from motor power to Newtons. Allows the copter
to hover against gravity when the motor is at hover_throttle to hover against gravity when the motor is at hover_throttle
normalized to hover at 1500RPM at 5 deg collective. normalized to hover at 1500RPM at 5 deg collective.
*/ */
thrust_scale = (mass * GRAVITY_MSS) / (hover_coll * sq(157.0f)); thrust_scale = (mass * GRAVITY_MSS) / (hover_coll * sq(nominal_rpm * 2.0f * M_PI / 60.0f));
// calculates tail rotor thrust to overcome rotor torque using the lean angle in a hover // calculates tail rotor thrust to overcome rotor torque using the lean angle in a hover
torque_scale = 0.83f * mass * GRAVITY_MSS * sinf(radians(hover_lean)) * tr_dist / (hover_coll * sq(157.0f)); torque_scale = 0.83f * mass * GRAVITY_MSS * sinf(radians(hover_lean)) * tr_dist / (hover_coll * sq(nominal_rpm * 2.0f * M_PI / 60.0f));
// torque with zero collective pitch. Percentage of total hover torque is based on full scale helicopters. // torque with zero collective pitch. Percentage of total hover torque is based on full scale helicopters.
torque_mpog = 0.17f * mass * GRAVITY_MSS * sinf(radians(hover_lean)) * tr_dist / sq(157.0f); torque_mpog = 0.17f * mass * GRAVITY_MSS * sinf(radians(hover_lean)) * tr_dist / sq(nominal_rpm * 2.0f * M_PI / 60.0f);
frame_height = 0.1; frame_height = 0.1;
if (strstr(frame_str, "-dual")) {
frame_type = HELI_FRAME_DUAL;
} else if (strstr(frame_str, "-compound")) {
frame_type = HELI_FRAME_COMPOUND;
} else {
frame_type = HELI_FRAME_CONVENTIONAL;
}
gas_heli = (strstr(frame_str, "-gas") != nullptr); gas_heli = (strstr(frame_str, "-gas") != nullptr);
ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
@ -110,8 +122,8 @@ void Helicopter::update(const struct sitl_input &input)
case HELI_FRAME_CONVENTIONAL: { case HELI_FRAME_CONVENTIONAL: {
// simulate a traditional helicopter // simulate a traditional helicopter
float Ma1s = 522.0f; float Ma1s = 617.5f;
float Lb1s = 922.0f; float Lb1s = 3588.6f;
float Mu = 0.003f; float Mu = 0.003f;
float Lv = -0.006; float Lv = -0.006;
float Xu = -0.125; float Xu = -0.125;
@ -131,13 +143,13 @@ void Helicopter::update(const struct sitl_input &input)
torque_effect_accel = -1 * sq(rpm[0] * 0.104667f) * (torque_mpog + torque_scale * fabsf(coll)) / izz; torque_effect_accel = -1 * sq(rpm[0] * 0.104667f) * (torque_mpog + torque_scale * fabsf(coll)) / izz;
// Calculate rotor tip path plane angle // Calculate rotor tip path plane angle
float roll_cyclic = (swash1 - swash2) / cyclic_scalar; float roll_cyclic = 1.283 * (swash1 - swash2) / cyclic_scalar;
float pitch_cyclic = ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar; float pitch_cyclic = 1.48 * ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar;
Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic); Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic);
update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt); update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt);
float yaw_cmd = 2.0f * tail_rotor - 1.0f; // convert range to -1 to 1 float yaw_cmd = 2.0f * tail_rotor - 1.0f; // convert range to -1 to 1
float tail_rotor_torque = (21.6f * 2.96f * yaw_cmd - 2.96f * gyro.z) * sq(rpm[0] * 0.104667f) / sq(157.0f); float tail_rotor_torque = (21.6f * 2.96f * yaw_cmd - 2.96f * gyro.z) * sq(rpm[0]/nominal_rpm);
float tail_rotor_thrust = -1.0f * tail_rotor_torque * izz / tr_dist; //right pedal produces left body accel float tail_rotor_thrust = -1.0f * tail_rotor_torque * izz / tr_dist; //right pedal produces left body accel
// rotational acceleration, in rad/s/s, in body frame // rotational acceleration, in rad/s/s, in body frame
@ -152,6 +164,57 @@ void Helicopter::update(const struct sitl_input &input)
break; break;
} }
case HELI_FRAME_BLADE: {
// simulate a Blade 360 helicopter. This model was taken from the following reference.
// Walker, J, Tishler, M, "Identification and Control Design of a Sub-Scale Flybarless Helicopter",
// Vertical Flight Society’s 77th Annual Forum & Technology Display, Virtual, May 10-14, 2021.
float Ma1s = 796.7f;
float Lb1s = 5115.2f;
float Mu = 2.7501f;
float Mv = -2.3039f;
float Lu = -28.7796f;
float Lv = -5.5376f;
float Xu = -0.2270f;
float Yv = -0.1852f;
float Yp = 0.2303f;
float Zw = -0.5910f;
float Nr = -2.0131f;
float Nw = 5.7574f;
float Nv = 1.7258f;
float Ncol = -32.4616f;
float Nped = 63.0040f;
float Zcol = -22.3239f;
float tail_rotor = (_servos_delayed[3]-1000) / 1000.0f;
// determine RPM
rpm[0] = update_rpm(motor_interlock, dt);
// collective adjusted for coll_min(1460) to coll_max(1740) as 0 to 1 with 1500 being zero thrust
float coll = 3.51 * ((swash1+swash2+swash3) / 3.0f - 0.5f);
// Calculate rotor tip path plane angle
float roll_cyclic = 1.283f * (swash1 - swash2);
float pitch_cyclic = 1.48f * ((swash1+swash2) / 2.0f - swash3);
Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic);
update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt);
float yaw_cmd = 1.45f * (2.0f * tail_rotor - 1.0f); // convert range to -1 to 1
// rotational acceleration, in rad/s/s, in body frame
rot_accel.x = _tpp_angle.x * Lb1s + Lu * velocity_air_bf.x + Lv * velocity_air_bf.y;
rot_accel.y = _tpp_angle.y * Ma1s + Mu * velocity_air_bf.x + Mv * velocity_air_bf.y;
rot_accel.z = Nv * velocity_air_bf.y + Nr * gyro.z + sq(rpm[0]/nominal_rpm) * Nped * yaw_cmd + Nw * velocity_air_bf.z + sq(rpm[0]/nominal_rpm) * Ncol * (coll - 0.5f);
lateral_y_thrust = GRAVITY_MSS * _tpp_angle.x + Yv * velocity_air_bf.y + Yp * gyro.x - 3.2 * 0.01745 * GRAVITY_MSS;
lateral_x_thrust = -1.0f * GRAVITY_MSS * _tpp_angle.y + Xu * velocity_air_bf.x;
float vertical_thrust = Zcol * coll * sq(rpm[0]/nominal_rpm) + velocity_air_bf.z * Zw;
accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, vertical_thrust);
break;
}
case HELI_FRAME_DUAL: { case HELI_FRAME_DUAL: {
// simulate a tandem helicopter // simulate a tandem helicopter
thrust_scale = (mass * GRAVITY_MSS) / hover_throttle; thrust_scale = (mass * GRAVITY_MSS) / hover_throttle;
@ -188,7 +251,7 @@ void Helicopter::update(const struct sitl_input &input)
air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity); air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
// simulate rotor speed // simulate rotor speed
rpm[0] = thrust * 1300; rpm[0] = thrust * nominal_rpm;
// scale thrust to newtons // scale thrust to newtons
thrust *= thrust_scale; thrust *= thrust_scale;
@ -202,8 +265,8 @@ void Helicopter::update(const struct sitl_input &input)
case HELI_FRAME_COMPOUND: { case HELI_FRAME_COMPOUND: {
// simulate a compound helicopter // simulate a compound helicopter
float Ma1s = 522.0f; float Ma1s = 617.5f;
float Lb1s = 922.0f; float Lb1s = 3588.6f;
float Mu = 0.003f; float Mu = 0.003f;
float Lv = -0.006; float Lv = -0.006;
float Xu = -0.125; float Xu = -0.125;
@ -221,8 +284,8 @@ void Helicopter::update(const struct sitl_input &input)
torque_effect_accel = -1 * sq(rpm[0] * 0.104667f) * (torque_mpog + torque_scale * fabsf(coll)) / izz; torque_effect_accel = -1 * sq(rpm[0] * 0.104667f) * (torque_mpog + torque_scale * fabsf(coll)) / izz;
// Calculate rotor tip path plane angle // Calculate rotor tip path plane angle
float roll_cyclic = (swash1 - swash2) / cyclic_scalar; float roll_cyclic = 1.283 * (swash1 - swash2) / cyclic_scalar;
float pitch_cyclic = ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar; float pitch_cyclic = 1.48 * ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar;
Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic); Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic);
update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt); update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt);
@ -232,8 +295,8 @@ void Helicopter::update(const struct sitl_input &input)
float left_thruster_cmd = 2.0f * (_servos_delayed[4]-1000) / 1000.0f - 1.0f; float left_thruster_cmd = 2.0f * (_servos_delayed[4]-1000) / 1000.0f - 1.0f;
// assume torque from each thruster only half of normal tailrotor since thrusters 1/2 distance from cg // assume torque from each thruster only half of normal tailrotor since thrusters 1/2 distance from cg
float right_thruster_torque = (-0.5f * 21.6f * 2.96f * right_thruster_cmd - 2.96f * gyro.z) * sq(rpm[0] * 0.104667f) / sq(157.0f); float right_thruster_torque = (-0.5f * 21.6f * 2.96f * right_thruster_cmd - 2.96f * gyro.z) * sq(rpm[0] / nominal_rpm);
float left_thruster_torque = (0.5f * 21.6f * 2.96f * left_thruster_cmd - 2.96f * gyro.z) * sq(rpm[0] * 0.104667f) / sq(157.0f); float left_thruster_torque = (0.5f * 21.6f * 2.96f * left_thruster_cmd - 2.96f * gyro.z) * sq(rpm[0] / nominal_rpm);
float right_thruster_force = -1.0f * right_thruster_torque * izz / (0.5f * tr_dist); float right_thruster_force = -1.0f * right_thruster_torque * izz / (0.5f * tr_dist);
float left_thruster_force = left_thruster_torque * izz / (0.5f * tr_dist); float left_thruster_force = left_thruster_torque * izz / (0.5f * tr_dist);
@ -265,13 +328,31 @@ void Helicopter::update(const struct sitl_input &input)
void Helicopter::update_rotor_dynamics(Vector3f gyros, Vector2f ctrl_pos, Vector2f &tpp_angle, float dt) void Helicopter::update_rotor_dynamics(Vector3f gyros, Vector2f ctrl_pos, Vector2f &tpp_angle, float dt)
{ {
float tf_inv = 1.0f / 0.07135f; float tf_inv;
float Lfa1s = 0.83641f; float Lfa1s;
float Mfb1s = -0.89074f; float Mfb1s;
float Lflt = 1.7869f; float Lflt;
float Lflg = -0.39394f; float Lflg;
float Mflt = 0.46231f; float Mflt;
float Mflg = 2.4099f; float Mflg;
if (frame_type == HELI_FRAME_BLADE) {
tf_inv = 1.0f / 0.0353f;
Lfa1s = 1.0477f;
Mfb1s = -1.0057f;
Lflt = 0.2375f;
Lflg = -0.0286f;
Mflt = 0.0344f;
Mflg = 0.2292f;
} else {
tf_inv = 1.0f / 0.068232f;
Lfa1s = 1.2963f;
Mfb1s = -1.3402f;
Lflt = 1.7635f;
Lflg = -0.61171f;
Mflt = 0.52454f;
Mflg = 1.9432f;
}
float b1s_dot = -1 * gyro.x - tf_inv * tpp_angle.x + tf_inv * (Lfa1s * tpp_angle.y + Lflt * ctrl_pos.x + Lflg * ctrl_pos.y); float b1s_dot = -1 * gyro.x - tf_inv * tpp_angle.x + tf_inv * (Lfa1s * tpp_angle.y + Lflt * ctrl_pos.x + Lflg * ctrl_pos.y);
float a1s_dot = -1 * gyro.y - tf_inv * tpp_angle.y + tf_inv * (Mfb1s * tpp_angle.x + Mflt * ctrl_pos.x + Mflg * ctrl_pos.y); float a1s_dot = -1 * gyro.y - tf_inv * tpp_angle.y + tf_inv * (Mfb1s * tpp_angle.x + Mflt * ctrl_pos.x + Mflg * ctrl_pos.y);
@ -301,7 +382,7 @@ float Helicopter::update_rpm(bool interlock, float dt)
} }
} }
return 1500.0f * constrain_float(rotor_runup_output,0.0f,1.0f); return nominal_rpm * constrain_float(rotor_runup_output,0.0f,1.0f);
} }

6
libraries/SITL/SIM_Helicopter.h
View File

@ -79,13 +79,15 @@ private:
float torque_mpog; float torque_mpog;
float hover_coll = 5.0f; float hover_coll = 5.0f;
bool motor_interlock; bool motor_interlock;
uint8_t _time_delay = 30; uint8_t _time_delay;
enum frame_types { enum frame_types {
HELI_FRAME_CONVENTIONAL, HELI_FRAME_CONVENTIONAL,
HELI_FRAME_DUAL, HELI_FRAME_DUAL,
HELI_FRAME_COMPOUND HELI_FRAME_COMPOUND,
HELI_FRAME_BLADE
} frame_type = HELI_FRAME_CONVENTIONAL; } frame_type = HELI_FRAME_CONVENTIONAL;
bool gas_heli = false; bool gas_heli = false;
float nominal_rpm;
}; };
} // namespace SITL } // namespace SITL