From 9c2d43f6c42f5cbcaf3f99e81dc8d9d3ce118a07 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Thu, 4 Apr 2024 21:14:42 -0400 Subject: [PATCH] SITL: improve dual heli dynamics model --- libraries/SITL/SIM_Helicopter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index f1ba61dc57..28727829e7 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -33,7 +33,7 @@ Helicopter::Helicopter(const char *frame_str) : _time_delay = 30; nominal_rpm = 1300; mass = 9.08f; - iyy = 0.2f; + iyy = 5.0f; } else if (strstr(frame_str, "-compound")) { frame_type = HELI_FRAME_COMPOUND; _time_delay = 50; @@ -218,7 +218,7 @@ void Helicopter::update(const struct sitl_input &input) case HELI_FRAME_DUAL: { - float Ma1s = 617.5f; + float Ma1s = 617.5f/5.0f; float Lb1s = 3588.6f; float Mu = 0.003f; float Lv = -0.006f; @@ -259,7 +259,7 @@ void Helicopter::update(const struct sitl_input &input) // rotational acceleration, in rad/s/s, in body frame rot_accel.x = (_tpp_angle_1.x + _tpp_angle_2.x) * Lb1s + Lv * velocity_air_bf.y; - rot_accel.y = (_tpp_angle_1.y + _tpp_angle_2.y) * Ma1s + (thrust_1 - thrust_2) * hub_dist / iyy + Mu * velocity_air_bf.x + hub_dist * gyro.y * Zw; + rot_accel.y = (_tpp_angle_1.y + _tpp_angle_2.y) * Ma1s + (thrust_1 - thrust_2) * hub_dist / iyy + Mu * velocity_air_bf.x; rot_accel.z = (_tpp_angle_1.x * thrust_1 - _tpp_angle_2.x * thrust_2) * hub_dist / (iyy * 2.0f) - 0.5f * gyro.z; lateral_y_thrust = GRAVITY_MSS * (_tpp_angle_1.x + _tpp_angle_2.x) + Yv * velocity_air_bf.y;