Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : helicopter simulator class
17 : */
18 :
19 : #include "SIM_Helicopter.h"
20 :
21 : #include <stdio.h>
22 : #include <GCS_MAVLink/GCS.h>
23 :
24 : namespace SITL {
25 :
26 80 : Helicopter::Helicopter(const char *frame_str) :
27 80 : Aircraft(frame_str)
28 : {
29 80 : mass = 4.54f;
30 :
31 80 : if (strstr(frame_str, "-dual")) {
32 0 : frame_type = HELI_FRAME_DUAL;
33 0 : _time_delay = 30;
34 0 : nominal_rpm = 1300;
35 0 : mass = 9.08f;
36 0 : iyy = 5.0f;
37 80 : } else if (strstr(frame_str, "-compound")) {
38 0 : frame_type = HELI_FRAME_COMPOUND;
39 0 : _time_delay = 50;
40 0 : nominal_rpm = 1500;
41 80 : } else if (strstr(frame_str, "-blade360")) {
42 1 : frame_type = HELI_FRAME_BLADE360;
43 1 : _time_delay = 40;
44 1 : nominal_rpm = 2100;
45 79 : } else if (strstr(frame_str, "-ddvptail")) {
46 1 : frame_type = HELI_FRAME_CONVENTIONAL_DDVPTAIL;
47 1 : _time_delay = 50;
48 1 : nominal_rpm = 1500;
49 78 : } else if (strstr(frame_str, "-ddfptail")) {
50 2 : frame_type = HELI_FRAME_CONVENTIONAL_DDFPTAIL;
51 2 : _time_delay = 50;
52 2 : nominal_rpm = 1500;
53 : } else {
54 76 : frame_type = HELI_FRAME_CONVENTIONAL;
55 76 : _time_delay = 50;
56 76 : nominal_rpm = 1500;
57 : }
58 :
59 : /*
60 : For conventional and compound
61 : scaling from motor power to Newtons. Allows the copter
62 : to hover against gravity when the motor is at hover_throttle
63 : normalized to hover at 1500RPM at 5 deg collective.
64 : */
65 80 : thrust_scale = (mass * GRAVITY_MSS) / (hover_coll * sq(nominal_rpm * 2.0f * M_PI / 60.0f));
66 :
67 : // torque with zero collective pitch. Percentage of total +hover torque is based on full scale helicopters.
68 80 : torque_mpog = 1.08f / sq(nominal_rpm * 2.0f * M_PI / 60.0f); // based on separate calculation
69 :
70 : // torque_max based on power being weight^3/2. Assuming thrust is linear with collective, it can be
71 : // determined that max torque would be 3 times hover torque. Then dividing by 1-MPOG_torq to get total torque.
72 80 : torque_max = ((mass * GRAVITY_MSS * sinf(radians(hover_lean)) * tr_dist - torque_mpog * sq(nominal_rpm * 2.0f * M_PI / 60.0f)) * powf(2.0f,1.5f) + torque_mpog * sq(nominal_rpm * 2.0f * M_PI / 60.0f)) / sq(nominal_rpm * 2.0f * M_PI / 60.0f);
73 :
74 : // calculates tail rotor thrust to overcome rotor torque using the lean angle in a hover
75 80 : torque_scale = (torque_max - torque_mpog) / powf(10.0f,1.5f);
76 :
77 80 : frame_height = 0.1;
78 :
79 80 : gas_heli = (strstr(frame_str, "-gas") != nullptr);
80 :
81 80 : ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
82 80 : lock_step_scheduled = true;
83 :
84 80 : motor_mask |= (1U<<0);
85 :
86 160 : battery.setup(sitl->batt_capacity_ah,
87 : default_battery_resistance_ohm,
88 80 : sitl->batt_voltage,
89 : ambient_outside_temperature_degC());
90 :
91 80 : }
92 :
93 : /*
94 : update the helicopter simulation by one time step
95 : */
96 9413166 : void Helicopter::update(const struct sitl_input &input)
97 : {
98 9413166 : const float dt = frame_time_us * 1.0e-6f;
99 :
100 : // get wind vector setup
101 9413166 : update_wind(input);
102 :
103 9413166 : motor_interlock = input.servos[7] > 1400;
104 :
105 9413166 : float rsc = constrain_float((input.servos[7]-1000) / 1000.0f, 0, 1);
106 :
107 9413166 : float thrust = 0;
108 9413166 : float thrust_1 = 0;
109 9413166 : float thrust_2 = 0;
110 9413166 : float eng_torque = 0;
111 9413166 : float lateral_x_thrust = 0;
112 9413166 : float lateral_y_thrust = 0;
113 :
114 9413166 : if (gas_heli) {
115 337532 : power_consumption_watts = 1.0f;
116 : } else {
117 9075634 : power_consumption_watts = 1000.0f;
118 : }
119 :
120 9413166 : update_battery();
121 :
122 9413166 : if (_time_delay == 0) {
123 0 : for (uint8_t i = 0; i < 6; i++) {
124 0 : _servos_delayed[i] = input.servos[i];
125 : }
126 9413166 : } else if (servos_stored_buffer == nullptr) {
127 80 : uint16_t buffer_size = constrain_int16(_time_delay, 1, 100) * 0.001f / dt;
128 80 : servos_stored_buffer = NEW_NOTHROW ObjectBuffer<servos_stored>(buffer_size);
129 4868 : while (servos_stored_buffer->space() != 0) {
130 4788 : push_to_buffer(input.servos);
131 : }
132 560 : for (uint8_t i = 0; i < 6; i++) {
133 480 : _servos_delayed[i] = input.servos[i];
134 : }
135 : } else {
136 9413086 : pull_from_buffer(_servos_delayed);
137 9413086 : push_to_buffer(input.servos);
138 : }
139 :
140 9413166 : float swash1 = (_servos_delayed[0]-1000) / 1000.0f;
141 9413166 : float swash2 = (_servos_delayed[1]-1000) / 1000.0f;
142 9413166 : float swash3 = (_servos_delayed[2]-1000) / 1000.0f;
143 :
144 9413166 : Vector3f rot_accel;
145 :
146 9413166 : switch (frame_type) {
147 8665485 : case HELI_FRAME_CONVENTIONAL: {
148 : // simulate a traditional helicopter
149 :
150 8665485 : float Ma1s = 617.5f;
151 8665485 : float Lb1s = 3588.6f;
152 8665485 : float Mu = 0.003f;
153 8665485 : float Lv = -0.006;
154 8665485 : float Xu = -0.125;
155 8665485 : float Yv = -0.375;
156 8665485 : float Zw = -2.25;
157 :
158 8665485 : float tail_rotor = (_servos_delayed[3]-1000) / 1000.0f;
159 :
160 : // thrust calculated based on 5 deg hover collective for 10lb aircraft at 1500RPM
161 8665485 : float coll = 50.0f * (swash1+swash2+swash3) / 3.0f - 25.0f;
162 8665485 : thrust = (rpm[0] / nominal_rpm) * thrust_scale * sq(nominal_rpm * 0.104667f) * coll;
163 :
164 : // determine RPM
165 8665485 : rpm[0] = update_rpm(rpm[0], rsc, eng_torque, coll, dt);
166 :
167 : // Calculate rotor tip path plane angle
168 8665485 : float roll_cyclic = 1.283 * (swash1 - swash2) / cyclic_scalar;
169 8665485 : float pitch_cyclic = 1.48 * ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar;
170 8665485 : Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic);
171 8665485 : update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt);
172 :
173 8665485 : float yaw_cmd = 2.0f * tail_rotor - 1.0f; // convert range to -1 to 1
174 8665485 : float tail_rotor_torque = (21.6f * 2.96f * yaw_cmd - 2.96f * gyro.z) * sq(rpm[0]/nominal_rpm);
175 8665485 : float tail_rotor_thrust = -1.0f * tail_rotor_torque * izz / tr_dist; //right pedal produces left body accel
176 :
177 : // rotational acceleration, in rad/s/s, in body frame
178 8665485 : rot_accel.x = _tpp_angle.x * Lb1s + Lv * velocity_air_bf.y;
179 8665485 : rot_accel.y = _tpp_angle.y * Ma1s + Mu * velocity_air_bf.x;
180 8665485 : rot_accel.z = tail_rotor_torque - eng_torque;
181 :
182 8665485 : lateral_y_thrust = tail_rotor_thrust / mass + GRAVITY_MSS * _tpp_angle.x + Yv * velocity_air_bf.y;
183 8665485 : lateral_x_thrust = -1.0f * GRAVITY_MSS * _tpp_angle.y + Xu * velocity_air_bf.x;
184 8665485 : accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass + velocity_air_bf.z * Zw);
185 :
186 8665485 : break;
187 : }
188 :
189 146519 : case HELI_FRAME_CONVENTIONAL_DDVPTAIL: {
190 : // simulate a traditional helicopter with DDVP tail. Simulates the tail rotor RSC by just applying a tailrsc multiplier to the tail rotor torque and thrust.
191 : // If the code is not configured correctly, the tail rotor will not produce any torque or thrust. The spool up is assumed to be the same as the main rotor.
192 : // the H_TAIL_SPEED parameter must be set to 100 for the tail rotor to operate correctly. The tail RSC is hard coded to SERVO channel 7.
193 :
194 146519 : float Ma1s = 617.5f;
195 146519 : float Lb1s = 3588.6f;
196 146519 : float Mu = 0.003f;
197 146519 : float Lv = -0.006;
198 146519 : float Xu = -0.125;
199 146519 : float Yv = -0.375;
200 146519 : float Zw = -2.25;
201 :
202 146519 : float tailrsc = constrain_float((input.servos[6]-1000) / 1000.0f, 0, 1);
203 :
204 146519 : float tail_rotor = (_servos_delayed[3]-1000) / 1000.0f;
205 :
206 : // thrust calculated based on 5 deg hover collective for 10lb aircraft at 1500RPM
207 146519 : float coll = 50.0f * (swash1+swash2+swash3) / 3.0f - 25.0f;
208 146519 : thrust = (rpm[0] / nominal_rpm) * thrust_scale * sq(nominal_rpm * 0.104667f) * coll;
209 :
210 : // determine RPM
211 146519 : rpm[0] = update_rpm(rpm[0], rsc, eng_torque, coll, dt);
212 :
213 : // Calculate rotor tip path plane angle
214 146519 : float roll_cyclic = 1.283 * (swash1 - swash2) / cyclic_scalar;
215 146519 : float pitch_cyclic = 1.48 * ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar;
216 146519 : Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic);
217 146519 : update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt);
218 :
219 146519 : float yaw_cmd = 2.0f * tail_rotor - 1.0f; // convert range to -1 to 1
220 146519 : float tail_rotor_torque = (21.6f * 2.96f * yaw_cmd - 2.96f * gyro.z) * tailrsc * sq(rpm[0]/nominal_rpm);
221 146519 : float tail_rotor_thrust = -1.0f * tail_rotor_torque * izz / tr_dist; //right pedal produces left body accel
222 :
223 : // rotational acceleration, in rad/s/s, in body frame
224 146519 : rot_accel.x = _tpp_angle.x * Lb1s + Lv * velocity_air_bf.y;
225 146519 : rot_accel.y = _tpp_angle.y * Ma1s + Mu * velocity_air_bf.x;
226 146519 : rot_accel.z = tail_rotor_torque - eng_torque;
227 :
228 146519 : lateral_y_thrust = tail_rotor_thrust / mass + GRAVITY_MSS * _tpp_angle.x + Yv * velocity_air_bf.y;
229 146519 : lateral_x_thrust = -1.0f * GRAVITY_MSS * _tpp_angle.y + Xu * velocity_air_bf.x;
230 146519 : accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass + velocity_air_bf.z * Zw);
231 :
232 146519 : break;
233 : }
234 :
235 458692 : case HELI_FRAME_CONVENTIONAL_DDFPTAIL: {
236 : // simulate a traditional helicopter with DDFP tail. Simulates the tail rotor by just applying a multiplier to the tail rotor command.
237 : // If the code is not configured correctly, the tail rotor will not produce any torque or thrust.
238 :
239 458692 : float Ma1s = 617.5f;
240 458692 : float Lb1s = 3588.6f;
241 458692 : float Mu = 0.003f;
242 458692 : float Lv = -0.006;
243 458692 : float Xu = -0.125;
244 458692 : float Yv = -0.375;
245 458692 : float Zw = -2.25;
246 :
247 : // float tailrsc = constrain_float((input.servos[6]-1000) / 1000.0f, 0, 1);
248 458692 : float tail_rotor = (input.servos[6]-1000) / 1000.0f;
249 :
250 : // thrust calculated based on 5 deg hover collective for 10lb aircraft at 1500RPM
251 458692 : float coll = 50.0f * (swash1+swash2+swash3) / 3.0f - 25.0f;
252 458692 : thrust = (rpm[0] / nominal_rpm) * thrust_scale * sq(nominal_rpm * 0.104667f) * coll;
253 :
254 : // determine RPM
255 458692 : rpm[0] = update_rpm(rpm[0], rsc, eng_torque, coll, dt);
256 :
257 : // Calculate rotor tip path plane angle
258 458692 : float roll_cyclic = 1.283 * (swash1 - swash2) / cyclic_scalar;
259 458692 : float pitch_cyclic = 1.48 * ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar;
260 458692 : Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic);
261 458692 : update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt);
262 :
263 : // tail rotor output is modified by the ratio of current battery voltage to max battery voltage
264 458692 : float tail_rotor_torque = (21.6f * 2.96f - 2.96f * gyro.z) * sq(tail_rotor * battery_voltage / sitl->batt_voltage);
265 458692 : float tail_rotor_thrust = -1.0f * tail_rotor_torque * izz / tr_dist; //right pedal produces left body accel
266 :
267 : // rotational acceleration, in rad/s/s, in body frame
268 458692 : rot_accel.x = _tpp_angle.x * Lb1s + Lv * velocity_air_bf.y;
269 458692 : rot_accel.y = _tpp_angle.y * Ma1s + Mu * velocity_air_bf.x;
270 458692 : rot_accel.z = tail_rotor_torque - eng_torque;
271 :
272 458692 : lateral_y_thrust = tail_rotor_thrust / mass + GRAVITY_MSS * _tpp_angle.x + Yv * velocity_air_bf.y;
273 458692 : lateral_x_thrust = -1.0f * GRAVITY_MSS * _tpp_angle.y + Xu * velocity_air_bf.x;
274 458692 : accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass + velocity_air_bf.z * Zw);
275 :
276 458692 : break;
277 : }
278 :
279 142470 : case HELI_FRAME_BLADE360: {
280 : // simulate a Blade 360 helicopter. This model was taken from the following reference.
281 : // Walker, J, Tishler, M, "Identification and Control Design of a Sub-Scale Flybarless Helicopter",
282 : // Vertical Flight Society’s 77th Annual Forum & Technology Display, Virtual, May 10-14, 2021.
283 :
284 142470 : float Ma1s = 796.7f;
285 142470 : float Lb1s = 5115.2f;
286 142470 : float Mu = 2.7501f;
287 142470 : float Mv = -2.3039f;
288 142470 : float Lu = -28.7796f;
289 142470 : float Lv = -5.5376f;
290 142470 : float Xu = -0.2270f;
291 142470 : float Yv = -0.1852f;
292 142470 : float Yp = 0.2303f;
293 142470 : float Zw = -0.5910f;
294 142470 : float Nr = -2.0131f;
295 142470 : float Nw = 5.7574f;
296 142470 : float Nv = 1.7258f;
297 142470 : float Ncol = -32.4616f;
298 142470 : float Nped = 63.0040f;
299 142470 : float Zcol = -22.3239f;
300 :
301 142470 : float tail_rotor = (_servos_delayed[3]-1000) / 1000.0f;
302 :
303 : // collective adjusted for coll_min(1460) to coll_max(1740) as 0 to 1 with 1500 being zero thrust
304 142470 : float coll = 3.51 * ((swash1+swash2+swash3) / 3.0f - 0.5f);
305 :
306 : // determine RPM
307 142470 : rpm[0] = update_rpm(rpm[0], rsc, eng_torque, coll, dt);
308 :
309 : // Calculate rotor tip path plane angle
310 142470 : float roll_cyclic = 1.283f * (swash1 - swash2);
311 142470 : float pitch_cyclic = 1.48f * ((swash1+swash2) / 2.0f - swash3);
312 142470 : Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic);
313 142470 : update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt);
314 :
315 142470 : float yaw_cmd = 1.45f * (2.0f * tail_rotor - 1.0f); // convert range to -1 to 1
316 :
317 : // rotational acceleration, in rad/s/s, in body frame
318 142470 : rot_accel.x = _tpp_angle.x * Lb1s + Lu * velocity_air_bf.x + Lv * velocity_air_bf.y;
319 142470 : rot_accel.y = _tpp_angle.y * Ma1s + Mu * velocity_air_bf.x + Mv * velocity_air_bf.y;
320 142470 : 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);
321 :
322 142470 : lateral_y_thrust = GRAVITY_MSS * _tpp_angle.x + Yv * velocity_air_bf.y + Yp * gyro.x - 3.2 * 0.01745 * GRAVITY_MSS;
323 142470 : lateral_x_thrust = -1.0f * GRAVITY_MSS * _tpp_angle.y + Xu * velocity_air_bf.x;
324 142470 : float vertical_thrust = Zcol * coll * sq(rpm[0]/nominal_rpm) + velocity_air_bf.z * Zw;
325 142470 : accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, vertical_thrust);
326 :
327 142470 : break;
328 : }
329 :
330 0 : case HELI_FRAME_DUAL: {
331 :
332 0 : float Ma1s = 617.5f/5.0f;
333 0 : float Lb1s = 3588.6f;
334 0 : float Mu = 0.003f;
335 0 : float Lv = -0.006f;
336 0 : float Xu = -0.125f;
337 0 : float Yv = -0.375f;
338 0 : float Zw = -2.25f;
339 0 : float hub_dist = 1.8f; //meters
340 :
341 0 : float swash4 = (_servos_delayed[3]-1000) / 1000.0f;
342 0 : float swash5 = (_servos_delayed[4]-1000) / 1000.0f;
343 0 : float swash6 = (_servos_delayed[5]-1000) / 1000.0f;
344 :
345 : // Forward rotor is number 1
346 : // thrust calculated based on 5 deg hover collective for 10lb aircraft at 1500RPM
347 0 : float coll_1 = 50.0f * (swash1+swash2+swash3) / 3.0f - 25.0f;
348 :
349 : // Calculate rotor tip path plane angle
350 0 : float roll_cyclic_1 = 1.283 * (swash1 - swash2) / cyclic_scalar;
351 0 : float pitch_cyclic_1 = 1.48 * ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar;
352 0 : Vector2f ctrl_pos_1 = Vector2f(roll_cyclic_1, pitch_cyclic_1);
353 0 : update_rotor_dynamics(gyro, ctrl_pos_1, _tpp_angle_1, dt);
354 :
355 : // Aft rotor is number 2
356 : // thrust calculated based on 5 deg hover collective for 10lb aircraft at 1500RPM
357 0 : float coll_2 = 50.0f * (swash4+swash5+swash6) / 3.0f - 25.0f;
358 :
359 : // Calculate rotor tip path plane angle
360 0 : float roll_cyclic_2 = 1.283 * (swash4 - swash5) / cyclic_scalar;
361 0 : float pitch_cyclic_2 = 1.48 * ((swash4+swash5) / 2.0f - swash6) / cyclic_scalar;
362 0 : Vector2f ctrl_pos_2 = Vector2f(roll_cyclic_2, pitch_cyclic_2);
363 0 : update_rotor_dynamics(gyro, ctrl_pos_2, _tpp_angle_2, dt);
364 :
365 : // determine RPM
366 0 : rpm[0] = update_rpm(rpm[0], rsc, eng_torque, (coll_1 + coll_2) * 0.5f, dt);
367 :
368 0 : thrust_1 = 0.5f * thrust_scale * sq(rpm[0] * 0.104667f) * coll_1;
369 0 : thrust_2 = 0.5f * thrust_scale * sq(rpm[0] * 0.104667f) * coll_2;
370 :
371 : // rotational acceleration, in rad/s/s, in body frame
372 0 : rot_accel.x = (_tpp_angle_1.x + _tpp_angle_2.x) * Lb1s + Lv * velocity_air_bf.y;
373 0 : rot_accel.y = (_tpp_angle_1.y + _tpp_angle_2.y) * Ma1s + (thrust_1 - thrust_2) * hub_dist / iyy + Mu * velocity_air_bf.x;
374 0 : rot_accel.z = (_tpp_angle_1.x * thrust_1 - _tpp_angle_2.x * thrust_2) * hub_dist / (iyy * 2.0f) - 0.5f * gyro.z;
375 :
376 0 : lateral_y_thrust = GRAVITY_MSS * (_tpp_angle_1.x + _tpp_angle_2.x) + Yv * velocity_air_bf.y;
377 0 : lateral_x_thrust = -1.0f * GRAVITY_MSS * (_tpp_angle_1.y + _tpp_angle_2.y) + Xu * velocity_air_bf.x;
378 0 : accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -(thrust_1 + thrust_2) / mass + velocity_air_bf.z * Zw);
379 :
380 :
381 0 : break;
382 : }
383 :
384 0 : case HELI_FRAME_COMPOUND: {
385 : // simulate a compound helicopter
386 :
387 0 : float Ma1s = 617.5f;
388 0 : float Lb1s = 3588.6f;
389 0 : float Mu = 0.003f;
390 0 : float Lv = -0.006;
391 0 : float Xu = -0.125;
392 0 : float Yv = -0.375;
393 0 : float Zw = -2.25;
394 :
395 : // thrust calculated based on 5 deg hover collective for 10lb aircraft at 1500RPM
396 0 : float coll = 50.0f * (swash1+swash2+swash3) / 3.0f - 25.0f;
397 0 : thrust = thrust_scale * sq(rpm[0] * 0.104667f) * coll;
398 :
399 : // determine RPM
400 0 : rpm[0] = update_rpm(rpm[0], rsc, eng_torque, coll, dt);
401 :
402 : // Calculate rotor tip path plane angle
403 0 : float roll_cyclic = 1.283 * (swash1 - swash2) / cyclic_scalar;
404 0 : float pitch_cyclic = 1.48 * ((swash1+swash2) / 2.0f - swash3) / cyclic_scalar;
405 0 : Vector2f ctrl_pos = Vector2f(roll_cyclic, pitch_cyclic);
406 0 : update_rotor_dynamics(gyro, ctrl_pos, _tpp_angle, dt);
407 :
408 : // Calculate thruster yaw and forward thrust effects
409 : // Thruster command range -1 to 1. Positive is forward thrust for both
410 0 : float right_thruster_cmd = 2.0f * (_servos_delayed[3]-1000) / 1000.0f - 1.0f;
411 0 : float left_thruster_cmd = 2.0f * (_servos_delayed[4]-1000) / 1000.0f - 1.0f;
412 :
413 : // assume torque from each thruster only half of normal tailrotor since thrusters 1/2 distance from cg
414 0 : float right_thruster_torque = (-0.5f * 21.6f * 2.96f * right_thruster_cmd - 2.96f * gyro.z) * sq(rpm[0] / nominal_rpm);
415 0 : float left_thruster_torque = (0.5f * 21.6f * 2.96f * left_thruster_cmd - 2.96f * gyro.z) * sq(rpm[0] / nominal_rpm);
416 :
417 0 : float right_thruster_force = -1.0f * right_thruster_torque * izz / (0.5f * tr_dist);
418 0 : float left_thruster_force = left_thruster_torque * izz / (0.5f * tr_dist);
419 :
420 : // rotational acceleration, in rad/s/s, in body frame
421 0 : rot_accel.x = _tpp_angle.x * Lb1s + Lv * velocity_air_bf.y;
422 0 : rot_accel.y = _tpp_angle.y * Ma1s + Mu * velocity_air_bf.x;
423 0 : rot_accel.z = right_thruster_torque + left_thruster_torque - eng_torque;
424 :
425 0 : lateral_y_thrust = GRAVITY_MSS * _tpp_angle.x + Yv * velocity_air_bf.y;
426 0 : lateral_x_thrust = (right_thruster_force + left_thruster_force) / mass - GRAVITY_MSS * _tpp_angle.y + Xu * velocity_air_bf.x;
427 0 : accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass + velocity_air_bf.z * Zw);
428 :
429 0 : break;
430 : }
431 : }
432 :
433 :
434 9413166 : update_dynamics(rot_accel);
435 9413166 : update_external_payload(input);
436 :
437 : // update lat/lon/altitude
438 9413166 : update_position();
439 9413166 : time_advance();
440 :
441 : // update magnetic field
442 9413166 : update_mag_field_bf();
443 :
444 9413166 : }
445 :
446 9413166 : void Helicopter::update_rotor_dynamics(Vector3f gyros, Vector2f ctrl_pos, Vector2f &tpp_angle, float dt)
447 : {
448 :
449 : float tf_inv;
450 : float Lfa1s;
451 : float Mfb1s;
452 : float Lflt;
453 : float Lflg;
454 : float Mflt;
455 : float Mflg;
456 :
457 9413166 : if (frame_type == HELI_FRAME_BLADE360) {
458 142470 : tf_inv = 1.0f / 0.0353f;
459 142470 : Lfa1s = 1.0477f;
460 142470 : Mfb1s = -1.0057f;
461 142470 : Lflt = 0.2375f;
462 142470 : Lflg = -0.0286f;
463 142470 : Mflt = 0.0344f;
464 142470 : Mflg = 0.2292f;
465 9270696 : } else if (frame_type == HELI_FRAME_DUAL) { // remove coupling in rotor
466 0 : tf_inv = 1.0f / 0.068232f;
467 0 : Lfa1s = 0.0f;
468 0 : Mfb1s = 0.0f;
469 0 : Lflt = 1.7635f;
470 0 : Lflg = 0.0f;
471 0 : Mflt = 0.0f;
472 0 : Mflg = 1.9432f;
473 : } else {
474 9270696 : tf_inv = 1.0f / 0.068232f;
475 9270696 : Lfa1s = 1.2963f;
476 9270696 : Mfb1s = -1.3402f;
477 9270696 : Lflt = 1.7635f;
478 9270696 : Lflg = -0.61171f;
479 9270696 : Mflt = 0.52454f;
480 9270696 : Mflg = 1.9432f;
481 : }
482 :
483 9413166 : 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);
484 9413166 : 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);
485 :
486 9413166 : tpp_angle.x += b1s_dot * dt;
487 9413166 : tpp_angle.y += a1s_dot * dt;
488 :
489 9413166 : }
490 :
491 9413166 : float Helicopter::update_rpm(float curr_rpm, float throttle, float &engine_torque, float collective, float dt)
492 : {
493 : static float rotor_runup_output;
494 : static uint8_t motor_status;
495 : float accel_scale;
496 : float auto_ss_torque;
497 : float descent_torque;
498 : float rotor_torque;
499 : float rpm_dot;
500 : float rpm_engine;
501 :
502 : //use this to make rpm model more realistic
503 9413166 : accel_scale = 100.0f;
504 :
505 : // calculate aerodynamic rotor drag torque
506 9413166 : rotor_torque = (sq(curr_rpm * 0.104667f) * (torque_mpog + torque_scale * powf(fabsf(collective),1.5f))) / izz;
507 :
508 : // Calculate autorotation effect on rotor
509 9413166 : auto_ss_torque = sq(nominal_rpm * 0.104667f) * (torque_mpog + torque_scale * powf(fabsf(-1.0f),1.5f)) / izz;
510 9413166 : if (is_positive(velocity_air_bf.z)) {
511 2224619 : descent_torque = (velocity_air_bf.z - 5.3) * auto_ss_torque / 5.3f + auto_ss_torque;
512 : } else {
513 7188547 : descent_torque = 0.0f;
514 : }
515 :
516 : // calculate max engine torque
517 9413166 : float engine_torque_max = sq(nominal_rpm * 0.104667f) * torque_max / izz;
518 :
519 9413166 : if (gas_heli) {
520 :
521 : // calculate engine RPM. Based off of throttle and it matches rotor RPM at 30% throttle
522 337532 : rpm_engine = nominal_rpm * throttle / 0.3f;
523 :
524 : // calculate engine torque. extra 20% given to have a little extra power
525 337532 : engine_torque = 1.20f * throttle * engine_torque_max;
526 :
527 : // model clutch on gas heli
528 : float input_torque;
529 337532 : if (throttle >= 0.15f && rpm_engine > curr_rpm) {
530 162849 : input_torque = engine_torque;
531 : } else {
532 174683 : input_torque = 0.0f;
533 : }
534 :
535 : // help spool down quickly go to zero
536 337532 : if (throttle <= 0.15f && curr_rpm < 300) {
537 143310 : rpm_dot = - 40.0f;
538 143310 : if (curr_rpm <= 0.0f) {
539 125304 : rpm_dot = 0.0f;
540 125304 : curr_rpm = 0.0f;
541 : }
542 : } else {
543 194222 : rpm_dot = accel_scale * (input_torque + descent_torque - rotor_torque);
544 194222 : if (curr_rpm <= 0.0 && !is_positive(rpm_dot)) {
545 0 : rpm_dot = 0.0f;
546 0 : curr_rpm = 0.0f;
547 : }
548 : }
549 : } else {
550 9075634 : if (throttle > 0.25) {
551 5668643 : motor_status = 3; // throttle unlimited
552 3406991 : } else if (motor_status == 3 && throttle <= 0.25 && throttle > 0.15) {
553 1 : motor_status = 2; // autorotational window
554 3406990 : } else if (throttle <= 0.15) {
555 3366230 : motor_status = 1; // idle
556 : }
557 :
558 9075634 : if (battery_is_empty()) {
559 0 : motor_status = 1; // battery empty and motor shuts down prematurely
560 : }
561 :
562 9075634 : float runup_time = 8.0f;
563 9075634 : if (motor_status == 2) {
564 6003 : runup_time = 2.0f;
565 : }
566 :
567 : // ramp speed estimate towards control out
568 9075634 : float runup_increment = dt / runup_time;
569 9075634 : if (motor_status > 2) {
570 5668643 : if (rotor_runup_output < 1.0f) {
571 328283 : rotor_runup_output += runup_increment;
572 : } else {
573 5340360 : rotor_runup_output = 1.0f;
574 : }
575 5668643 : if (curr_rpm < nominal_rpm - 25.0f) {
576 339914 : accel_scale = 2000.0f / runup_time;
577 : }
578 : }else{
579 3406991 : if (rotor_runup_output > 0.0f) {
580 32526 : rotor_runup_output -= runup_increment * 10.0f; // make ramp down 10 times faster
581 : } else {
582 3374465 : rotor_runup_output = 0.0f;
583 : }
584 : }
585 :
586 : // calculate engine torque just for start up and shutdown of rotor
587 9075634 : engine_torque = 0.333f * rotor_runup_output * engine_torque_max;
588 :
589 : // manage input torque so descent torque combined with engine torque doesn't allow rotor to overspeed
590 : float input_torque;
591 9075634 : if (rotor_runup_output >= 1.0f && curr_rpm > nominal_rpm - 100.0f) {
592 : // want the rpm to seek the nominal rpm so set the input torque to only be that for nominal RPM
593 5331852 : input_torque = rotor_torque * sq(nominal_rpm / curr_rpm);
594 3743782 : } else if (rotor_runup_output <= 0.0f) {
595 3374503 : input_torque = descent_torque;
596 : } else {
597 369279 : input_torque = engine_torque + descent_torque;
598 : }
599 :
600 : // Help spool down quickly got to zero
601 9075634 : if (rotor_runup_output <= 0.0f && curr_rpm < 300) {
602 2993459 : rpm_dot = - 40.0f;
603 2993459 : if (curr_rpm <= 0.0f) {
604 2791909 : rpm_dot = 0.0f;
605 2791909 : curr_rpm = 0.0f;
606 : }
607 : } else {
608 6082175 : rpm_dot = accel_scale * (input_torque - rotor_torque);
609 : }
610 9075634 : engine_torque = input_torque;
611 : }
612 :
613 9413166 : curr_rpm += rpm_dot * dt;
614 :
615 9413166 : return curr_rpm;
616 :
617 : }
618 :
619 : // push servo input to buffer
620 9417874 : void Helicopter::push_to_buffer(const uint16_t servos_input[16])
621 : {
622 : servos_stored sample;
623 9417874 : sample.servo1 = servos_input[0];
624 9417874 : sample.servo2 = servos_input[1];
625 9417874 : sample.servo3 = servos_input[2];
626 9417874 : sample.servo4 = servos_input[3];
627 9417874 : sample.servo5 = servos_input[4];
628 9417874 : sample.servo6 = servos_input[5];
629 9417874 : servos_stored_buffer->push(sample);
630 :
631 9417874 : }
632 :
633 : // pull servo delay from buffer
634 9413086 : void Helicopter::pull_from_buffer(uint16_t servos_delayed[6])
635 : {
636 : servos_stored sample;
637 9413086 : if (!servos_stored_buffer->pop(sample)) {
638 : // no sample
639 0 : return;
640 : }
641 9413086 : servos_delayed[0] = sample.servo1;
642 9413086 : servos_delayed[1] = sample.servo2;
643 9413086 : servos_delayed[2] = sample.servo3;
644 9413086 : servos_delayed[3] = sample.servo4;
645 9413086 : servos_delayed[4] = sample.servo5;
646 9413086 : servos_delayed[5] = sample.servo6;
647 :
648 : }
649 :
650 9413166 : void Helicopter::update_battery()
651 : {
652 9413166 : battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah);
653 9413166 : battery_voltage = battery.get_voltage();
654 :
655 : // initially set power consumption for just running the servos and electronics.
656 9413166 : float power_watt = 1.0f;
657 9413166 : if (motor_interlock) {
658 : // set the power consumption for the rotors turning at operational speed.
659 5702010 : power_watt = power_consumption_watts;
660 : }
661 : // calculate current drawn from battery based on power and voltage. Use a minimum voltage of 0.1 to avoid divide by zero.
662 9413166 : battery_current = power_watt / MAX(battery_voltage, 0.1);
663 :
664 9413166 : const uint64_t now_us = AP_HAL::micros64();
665 9413166 : battery.consume_energy(battery_current, now_us);
666 9413166 : }
667 :
668 : } // namespace SITL
|
