diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index 2a11d7cc1f..8b73be4b6f 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -25,16 +25,20 @@ namespace SITL { Helicopter::Helicopter(const char *frame_str) : Aircraft(frame_str) { - mass = 2.13f; + mass = 4.54f; /* scaling from motor power to Newtons. Allows the copter to hover against gravity when the motor is at hover_throttle + normalized to hover at 1500RPM at 5 deg collective. */ - thrust_scale = (mass * GRAVITY_MSS) / hover_throttle; + thrust_scale = (mass * GRAVITY_MSS) / (hover_coll * sq(157.0f)); - // calculate lateral thrust ratio for tail rotor - tail_thrust_scale = sinf(radians(hover_lean)) * thrust_scale / yaw_zero; + // 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 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); frame_height = 0.1; @@ -55,12 +59,19 @@ Helicopter::Helicopter(const char *frame_str) : */ void Helicopter::update(const struct sitl_input &input) { + const float dt = frame_time_us * 1.0e-6f; + // get wind vector setup update_wind(input); + if (input.servos[7] > 1400) { + motor_interlock = true; + } else { + motor_interlock = false; + } + float rsc = constrain_float((input.servos[7]-1000) / 1000.0f, 0, 1); - // ignition only for gas helis - bool ignition_enabled = gas_heli?(input.servos[5] > 1500):true; + float rsc_scale = rsc/rsc_setpoint; float thrust = 0; float roll_rate = 0; @@ -70,38 +81,87 @@ void Helicopter::update(const struct sitl_input &input) float lateral_x_thrust = 0; float lateral_y_thrust = 0; - float swash1 = (input.servos[0]-1000) / 1000.0f; - float swash2 = (input.servos[1]-1000) / 1000.0f; - float swash3 = (input.servos[2]-1000) / 1000.0f; - if (!ignition_enabled) { - rsc = 0; + if (_time_delay == 0) { + for (uint8_t i = 0; i < 6; i++) { + _servos_delayed[i] = input.servos[i]; + } + } else if (servos_stored_buffer == nullptr) { + uint16_t buffer_size = constrain_int16(_time_delay, 1, 100) * 0.001f / dt; + servos_stored_buffer = new ObjectBuffer(buffer_size); + while (servos_stored_buffer->space() != 0) { + push_to_buffer(input.servos); + } + for (uint8_t i = 0; i < 6; i++) { + _servos_delayed[i] = input.servos[i]; + } + } else { + pull_from_buffer(_servos_delayed); + push_to_buffer(input.servos); } - float rsc_scale = rsc/rsc_setpoint; + + float swash1 = (_servos_delayed[0]-1000) / 1000.0f; + float swash2 = (_servos_delayed[1]-1000) / 1000.0f; + float swash3 = (_servos_delayed[2]-1000) / 1000.0f; + + + + Vector3f rot_accel; + Vector3f air_resistance; switch (frame_type) { case HELI_FRAME_CONVENTIONAL: { // simulate a traditional helicopter - float tail_rotor = (input.servos[3]-1000) / 1000.0f; + float Ma1s = 522.0f; + float Lb1s = 922.0f; + float Mu = 0.003f; + float Lv = -0.006; + float Xu = -0.125; + float Yv = -0.375; + float Zw = -0.375; - thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f; - torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel; + float tail_rotor = (_servos_delayed[3]-1000) / 1000.0f; - roll_rate = swash1 - swash2; - pitch_rate = (swash1+swash2) / 2.0f - swash3; - yaw_rate = tail_rotor - 0.5f; + // determine RPM + rpm[0] = update_rpm(motor_interlock, dt); + + // thrust calculated based on 5 deg hover collective for 10lb aircraft at 1500RPM + float coll = 50.0f * (swash1+swash2+swash3) / 3.0f - 25.0f; + thrust = thrust_scale * sq(rpm[0] * 0.104667f) * (0.25* (coll - hover_coll) + hover_coll); + + // Calculate main rotor torque effect on body + torque_effect_accel = -1 * sq(rpm[0] * 0.104667f) * (torque_mpog + torque_scale * fabsf(coll)) / izz; + + // Calculate rotor tip path plane angle + float roll_cyclic = (swash1 - swash2) / cyclic_scalar; + float pitch_cyclic = ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar; + Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic); + 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 tail_rotor_torque = (21.6f * 2.96f * yaw_cmd - 2.96f * gyro.z) * sq(rpm[0] * 0.104667f) / sq(157.0f); + 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 + rot_accel.x = _tpp_angle.x * Lb1s + Lv * velocity_air_bf.y; + rot_accel.y = _tpp_angle.y * Ma1s + Mu * velocity_air_bf.x; + rot_accel.z = tail_rotor_torque + torque_effect_accel; + + lateral_y_thrust = tail_rotor_thrust / mass + GRAVITY_MSS * _tpp_angle.x + Yv * velocity_air_bf.y; + lateral_x_thrust = -1.0f * GRAVITY_MSS * _tpp_angle.y + Xu * velocity_air_bf.x; + accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass + velocity_air_bf.z * Zw); - lateral_y_thrust = yaw_rate * rsc_scale * tail_thrust_scale; break; } case HELI_FRAME_DUAL: { // simulate a tandem helicopter + thrust_scale = (mass * GRAVITY_MSS) / hover_throttle; - float swash4 = (input.servos[3]-1000) / 1000.0f; - float swash5 = (input.servos[4]-1000) / 1000.0f; - float swash6 = (input.servos[5]-1000) / 1000.0f; + float swash4 = (_servos_delayed[3]-1000) / 1000.0f; + float swash5 = (_servos_delayed[4]-1000) / 1000.0f; + float swash6 = (_servos_delayed[5]-1000) / 1000.0f; thrust = (rsc / rsc_setpoint) * (swash1+swash2+swash3+swash4+swash5+swash6) / 6.0f; torque_effect_accel = (rsc_scale + rsc / rsc_setpoint) * rotor_rot_accel * ((swash1+swash2+swash3) - (swash4+swash5+swash6)); @@ -109,56 +169,91 @@ void Helicopter::update(const struct sitl_input &input) roll_rate = (swash1-swash2) + (swash4-swash5); pitch_rate = (swash1+swash2+swash3) - (swash4+swash5+swash6); yaw_rate = (swash1-swash2) + (swash5-swash4); + + roll_rate *= rsc_scale; + pitch_rate *= rsc_scale; + yaw_rate *= rsc_scale; + + // rotational acceleration, in rad/s/s, in body frame + rot_accel.x = roll_rate * roll_rate_max; + rot_accel.y = pitch_rate * pitch_rate_max; + rot_accel.z = yaw_rate * yaw_rate_max; + + // rotational air resistance + rot_accel.x -= gyro.x * radians(5000.0) / terminal_rotation_rate; + rot_accel.y -= gyro.y * radians(5000.0) / terminal_rotation_rate; + rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate; + + // torque effect on tail + rot_accel.z += torque_effect_accel; + + // air resistance + air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity); + + // simulate rotor speed + rpm[0] = thrust * 1300; + + // scale thrust to newtons + thrust *= thrust_scale; + + accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass); + accel_body += dcm.transposed() * air_resistance; + break; } case HELI_FRAME_COMPOUND: { // simulate a compound helicopter - float right_rotor = (input.servos[3]-1000) / 1000.0f; - float left_rotor = (input.servos[4]-1000) / 1000.0f; + float Ma1s = 522.0f; + float Lb1s = 922.0f; + float Mu = 0.003f; + float Lv = -0.006; + float Xu = -0.125; + float Yv = -0.375; + float Zw = -0.375; - thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f; - torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel; + // determine RPM + rpm[0] = update_rpm(motor_interlock, dt); - roll_rate = swash1 - swash2; - pitch_rate = (swash1+swash2) / 2.0f - swash3; - yaw_rate = right_rotor - left_rotor; + // thrust calculated based on 5 deg hover collective for 10lb aircraft at 1500RPM + float coll = 50.0f * (swash1+swash2+swash3) / 3.0f - 25.0f; + thrust = thrust_scale * sq(rpm[0] * 0.104667f) * (0.25* (coll - hover_coll) + hover_coll); + + // Calculate main rotor torque effect on body + torque_effect_accel = -1 * sq(rpm[0] * 0.104667f) * (torque_mpog + torque_scale * fabsf(coll)) / izz; + + // Calculate rotor tip path plane angle + float roll_cyclic = (swash1 - swash2) / cyclic_scalar; + float pitch_cyclic = ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar; + Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic); + update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt); + + // Calculate thruster yaw and forward thrust effects + // Thruster command range -1 to 1. Positive is forward thrust for both + float right_thruster_cmd = 2.0f * (_servos_delayed[3]-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 + 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 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 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); + + // rotational acceleration, in rad/s/s, in body frame + rot_accel.x = _tpp_angle.x * Lb1s + Lv * velocity_air_bf.y; + rot_accel.y = _tpp_angle.y * Ma1s + Mu * velocity_air_bf.x; + rot_accel.z = right_thruster_torque + left_thruster_torque + torque_effect_accel; + + lateral_y_thrust = GRAVITY_MSS * _tpp_angle.x + Yv * velocity_air_bf.y; + lateral_x_thrust = (right_thruster_force + left_thruster_force) / mass - GRAVITY_MSS * _tpp_angle.y + Xu * velocity_air_bf.x; + accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass + velocity_air_bf.z * Zw); - lateral_x_thrust = (left_rotor+right_rotor-1) * rsc_scale * tail_thrust_scale; break; } } - roll_rate *= rsc_scale; - pitch_rate *= rsc_scale; - yaw_rate *= rsc_scale; - - // rotational acceleration, in rad/s/s, in body frame - Vector3f rot_accel; - rot_accel.x = roll_rate * roll_rate_max; - rot_accel.y = pitch_rate * pitch_rate_max; - rot_accel.z = yaw_rate * yaw_rate_max; - - // rotational air resistance - rot_accel.x -= gyro.x * radians(5000.0) / terminal_rotation_rate; - rot_accel.y -= gyro.y * radians(5000.0) / terminal_rotation_rate; - rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate; - - // torque effect on tail - rot_accel.z += torque_effect_accel; - - // air resistance - Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity); - - // simulate rotor speed - rpm[0] = thrust * 1300; - - // scale thrust to newtons - thrust *= thrust_scale; - - accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass); - accel_body += dcm.transposed() * air_resistance; update_dynamics(rot_accel); @@ -170,4 +265,75 @@ void Helicopter::update(const struct sitl_input &input) update_mag_field_bf(); } +void Helicopter::update_rotor_dynamics(Vector3f gyros, Vector2f ctrl_pos, Vector2f &tpp_angle, float dt) +{ + + float tf_inv = 1.0f / 0.07135f; + float Lfa1s = 0.83641f; + float Mfb1s = -0.89074f; + float Lflt = 1.7869f; + float Lflg = -0.39394f; + float Mflt = 0.46231f; + float Mflg = 2.4099f; + + 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); + + tpp_angle.x += b1s_dot * dt; + tpp_angle.y += a1s_dot * dt; + +} + +float Helicopter::update_rpm(bool interlock, float dt) +{ + static float rotor_runup_output; + float runup_time = 8.0f; + // ramp speed estimate towards control out + float runup_increment = dt / runup_time; + if (interlock) { + if (rotor_runup_output < 1.0f) { + rotor_runup_output += runup_increment; + } else { + rotor_runup_output = 1.0f; + } + }else{ + if (rotor_runup_output > 0.0f) { + rotor_runup_output -= runup_increment; + } else { + rotor_runup_output = 0.0f; + } + } + + return 1500.0f * constrain_float(rotor_runup_output,0.0f,1.0f); + +} + +// push servo input to buffer +void Helicopter::push_to_buffer(const uint16_t servos_input[16]) +{ + servos_stored sample; + sample.servo1 = servos_input[0]; + sample.servo2 = servos_input[1]; + sample.servo3 = servos_input[2]; + sample.servo4 = servos_input[3]; + sample.servo5 = servos_input[4]; + sample.servo6 = servos_input[5]; + servos_stored_buffer->push(sample); + +} + +// pull servo delay from buffer +void Helicopter::pull_from_buffer(uint16_t servos_delayed[6]) +{ + servos_stored sample; + servos_stored_buffer->pop(sample); + servos_delayed[0] = sample.servo1; + servos_delayed[1] = sample.servo2; + servos_delayed[2] = sample.servo3; + servos_delayed[3] = sample.servo4; + servos_delayed[4] = sample.servo5; + servos_delayed[5] = sample.servo6; + +} + } // namespace SITL diff --git a/libraries/SITL/SIM_Helicopter.h b/libraries/SITL/SIM_Helicopter.h index 580b27b924..95c4dee187 100644 --- a/libraries/SITL/SIM_Helicopter.h +++ b/libraries/SITL/SIM_Helicopter.h @@ -37,19 +37,49 @@ public: return new Helicopter(frame_str); } +protected: + + void update_rotor_dynamics(Vector3f gyros, Vector2f ctrl_pos, Vector2f &tpp_angle, float dt); + float update_rpm(bool interlock, float dt); + + // buffers to provide time delay + struct servos_stored { + uint16_t servo1; + uint16_t servo2; + uint16_t servo3; + uint16_t servo4; + uint16_t servo5; + uint16_t servo6; + }; + uint16_t _servos_delayed[6]; + ObjectBuffer *servos_stored_buffer; + void push_to_buffer(const uint16_t servos_input[16]); + void pull_from_buffer(uint16_t servos_delayed[6]); + + private: float terminal_rotation_rate = 4*radians(360.0f); - float hover_throttle = 0.65f; - float terminal_velocity = 40; - float hover_lean = 3.0f; + float hover_throttle = 0.5f; + float terminal_velocity = 80; + float hover_lean = 3.2f; float yaw_zero = 0.1f; float rotor_rot_accel = radians(20); float roll_rate_max = radians(1400); float pitch_rate_max = radians(1400); float yaw_rate_max = radians(1400); float rsc_setpoint = 0.8f; + float izz = 0.2f; + float tr_dist = 0.85f; + float tr_accel_max = 50.0f; //rad/s/s + float cyclic_scalar = 7.2; // converts swashplate servo ouputs to cyclic blade pitch float thrust_scale; float tail_thrust_scale; + Vector2f _tpp_angle; + float torque_scale; + float torque_mpog; + float hover_coll = 5.0f; + bool motor_interlock; + uint8_t _time_delay = 30; enum frame_types { HELI_FRAME_CONVENTIONAL, HELI_FRAME_DUAL,