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 : #include <stdlib.h>
17 : #include <AP_HAL/AP_HAL.h>
18 : #include "AP_MotorsHeli.h"
19 : #include <GCS_MAVLink/GCS.h>
20 : #include <AP_Logger/AP_Logger.h>
21 : #include <AC_Autorotation/RSC_Autorotation.h>
22 :
23 : extern const AP_HAL::HAL& hal;
24 :
25 : const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
26 :
27 : // 1 was ROL_MAX which has been replaced by CYC_MAX
28 :
29 : // 2 was PIT_MAX which has been replaced by CYC_MAX
30 :
31 : // @Param: COL_MIN
32 : // @DisplayName: Minimum Collective Pitch
33 : // @Description: Lowest possible servo position in PWM microseconds for the swashplate
34 : // @Range: 1000 2000
35 : // @Units: PWM
36 : // @Increment: 1
37 : // @User: Standard
38 : AP_GROUPINFO("COL_MIN", 3, AP_MotorsHeli, _collective_min, AP_MOTORS_HELI_COLLECTIVE_MIN),
39 :
40 : // @Param: COL_MAX
41 : // @DisplayName: Maximum Collective Pitch
42 : // @Description: Highest possible servo position in PWM microseconds for the swashplate
43 : // @Range: 1000 2000
44 : // @Units: PWM
45 : // @Increment: 1
46 : // @User: Standard
47 : AP_GROUPINFO("COL_MAX", 4, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX),
48 :
49 : // index 5 was COL_MID. Do not use this index in the future.
50 :
51 : // @Param: SV_MAN
52 : // @DisplayName: Manual Servo Mode
53 : // @Description: Manual servo override for swash set-up. Must be 0 (Disabled) for flight!
54 : // @Values: 0:Disabled,1:Passthrough,2:Max collective,3:Zero thrust collective,4:Min collective
55 : // @User: Standard
56 : AP_GROUPINFO("SV_MAN", 6, AP_MotorsHeli, _servo_mode, SERVO_CONTROL_MODE_AUTOMATED),
57 :
58 : // indices 7 and 8 were RSC parameters which were moved to RSC library. Do not use these indices in the future.
59 :
60 : // index 9 was LAND_COL_MIN. Do not use this index in the future.
61 :
62 : // indices 10-13 were RSC parameters which were moved to RSC library. Do not use these indices in the future.
63 :
64 : // index 14 was RSC_POWER_LOW. Do not use this index in the future.
65 :
66 : // index 15 was RSC_POWER_HIGH. Do not use this index in the future.
67 :
68 : // @Param: CYC_MAX
69 : // @DisplayName: Maximum Cyclic Pitch Angle
70 : // @Description: Maximum cyclic pitch angle of the swash plate. There are no units to this parameter. This should be adjusted to get the desired cyclic blade pitch for the pitch and roll axes. Typically this should be 6-7 deg (measured blade pitch angle difference between stick centered and stick max deflection.
71 : // @Range: 0 4500
72 : // @Increment: 100
73 : // @User: Standard
74 : AP_GROUPINFO("CYC_MAX", 16, AP_MotorsHeli, _cyclic_max, AP_MOTORS_HELI_SWASH_CYCLIC_MAX),
75 :
76 : // @Param: SV_TEST
77 : // @DisplayName: Boot-up Servo Test Cycles
78 : // @Description: Number of cycles to run servo test on boot-up
79 : // @Range: 0 10
80 : // @Increment: 1
81 : // @User: Standard
82 : AP_GROUPINFO("SV_TEST", 17, AP_MotorsHeli, _servo_test, 0),
83 :
84 : // index 18 was RSC_POWER_NEGC. Do not use this index in the future.
85 :
86 : // index 19 was RSC_SLEWRATE and was moved to RSC library. Do not use this index in the future.
87 :
88 : // indices 20 to 24 was throttle curve. Do not use this index in the future.
89 :
90 : // @Group: RSC_
91 : // @Path: AP_MotorsHeli_RSC.cpp
92 : AP_SUBGROUPINFO(_main_rotor, "RSC_", 25, AP_MotorsHeli, AP_MotorsHeli_RSC),
93 :
94 : // @Param: COL_HOVER
95 : // @DisplayName: Collective Hover Value
96 : // @Description: Collective needed to hover expressed as a number from 0 to 1 where 0 is H_COL_MIN and 1 is H_COL_MAX
97 : // @Range: 0.3 0.8
98 : // @User: Advanced
99 : AP_GROUPINFO("COL_HOVER", 26, AP_MotorsHeli, _collective_hover, AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT),
100 :
101 : // @Param: HOVER_LEARN
102 : // @DisplayName: Hover Value Learning
103 : // @Description: Enable/Disable automatic learning of hover collective
104 : // @Values: 0:Disabled, 1:Learn, 2:Learn and Save
105 : // @User: Advanced
106 : AP_GROUPINFO("HOVER_LEARN", 27, AP_MotorsHeli, _collective_hover_learn, HOVER_LEARN_AND_SAVE),
107 :
108 : // @Param: OPTIONS
109 : // @DisplayName: Heli_Options
110 : // @Description: Bitmask of heli options. Bit 0 changes how the pitch, roll, and yaw axis integrator term is managed for low speed and takeoff/landing. In AC 4.0 and earlier, scheme uses a leaky integrator for ground speeds less than 5 m/s and won't let the steady state integrator build above ILMI. The integrator is allowed to build to the ILMI value when it is landed. The other integrator management scheme bases integrator limiting on takeoff and landing. Whenever the aircraft is landed the integrator is set to zero. When the aicraft is airborne, the integrator is only limited by IMAX.
111 : // @Bitmask: 0:Use Leaky I
112 : // @User: Standard
113 : AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I),
114 :
115 : // @Param: COL_ANG_MIN
116 : // @DisplayName: Collective Blade Pitch Angle Minimum
117 : // @Description: Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN).
118 : // @Range: -20 0
119 : // @Units: deg
120 : // @Increment: 0.1
121 : // @User: Standard
122 : AP_GROUPINFO("COL_ANG_MIN", 29, AP_MotorsHeli, _collective_min_deg, AP_MOTORS_HELI_COLLECTIVE_MIN_DEG),
123 :
124 : // @Param: COL_ANG_MAX
125 : // @DisplayName: Collective Blade Pitch Angle Maximum
126 : // @Description: Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX).
127 : // @Range: 5 20
128 : // @Units: deg
129 : // @Increment: 0.1
130 : // @User: Standard
131 : AP_GROUPINFO("COL_ANG_MAX", 30, AP_MotorsHeli, _collective_max_deg, AP_MOTORS_HELI_COLLECTIVE_MAX_DEG),
132 :
133 : // @Param: COL_ZERO_THRST
134 : // @DisplayName: Collective Blade Pitch at Zero Thrust
135 : // @Description: Collective blade pitch angle at zero thrust in degrees. For symetric airfoil blades this value is zero deg. For chambered airfoil blades this value is typically negative.
136 : // @Range: -5 0
137 : // @Units: deg
138 : // @Increment: 0.1
139 : // @User: Standard
140 : AP_GROUPINFO("COL_ZERO_THRST", 31, AP_MotorsHeli, _collective_zero_thrust_deg, 0.0f),
141 :
142 : // @Param: COL_LAND_MIN
143 : // @DisplayName: Collective Blade Pitch Minimum when Landed
144 : // @Description: Minimum collective blade pitch angle when landed in degrees for non-manual collective modes (i.e. modes that use altitude hold).
145 : // @Range: -5 0
146 : // @Units: deg
147 : // @Increment: 0.1
148 : // @User: Standard
149 : AP_GROUPINFO("COL_LAND_MIN", 32, AP_MotorsHeli, _collective_land_min_deg, AP_MOTORS_HELI_COLLECTIVE_LAND_MIN),
150 :
151 : // @Group: RSC_AROT_
152 : // @Path: ../AC_Autorotation/RSC_Autorotation.cpp
153 : AP_SUBGROUPINFO(_main_rotor.autorotation, "RSC_AROT_", 33, AP_MotorsHeli, RSC_Autorotation),
154 :
155 : AP_GROUPEND
156 : };
157 :
158 : //
159 : // public methods
160 : //
161 :
162 : // init
163 83 : void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_type)
164 : {
165 : // remember frame class and type
166 83 : _frame_type = frame_type;
167 83 : _frame_class = frame_class;
168 :
169 : // set update rate
170 83 : set_update_rate(_speed_hz);
171 :
172 : // load boot-up servo test cycles into counter to be consumed
173 83 : _servo_test_cycle_counter = _servo_test;
174 :
175 : // ensure inputs are not passed through to servos on start-up
176 83 : _servo_mode.set(SERVO_CONTROL_MODE_AUTOMATED);
177 :
178 : // initialise radio passthrough for collective to middle
179 83 : _throttle_radio_passthrough = 0.5f;
180 :
181 : // initialise Servo/PWM ranges and endpoints
182 83 : init_outputs();
183 :
184 : // calculate all scalars
185 83 : calculate_scalars();
186 :
187 : // set flag to true so targets are initialized once aircraft is armed for first time
188 83 : _heliflags.init_targets_on_arming = true;
189 :
190 83 : _mav_type = MAV_TYPE_HELICOPTER;
191 83 : }
192 :
193 : // calculate_scalars - recalculates various scalers used.
194 3234062 : void AP_MotorsHeli::calculate_armed_scalars()
195 : {
196 3234062 : _main_rotor.configure_armed();
197 3234062 : }
198 :
199 : // output_min - sets servos to neutral point with motors stopped
200 140 : void AP_MotorsHeli::output_min()
201 : {
202 : // move swash to mid
203 140 : move_actuators(0.0f,0.0f,0.5f,0.0f);
204 :
205 : // _spool_state is enforced to SHUT_DOWN when _spool_desired is SHUT_DOWN.
206 140 : set_desired_spool_state(DesiredSpoolState::SHUT_DOWN);
207 140 : _spool_state = update_spool_state(_spool_desired);
208 140 : update_motor_control(_spool_desired);
209 :
210 140 : output_to_motors();
211 :
212 : // override limits flags
213 140 : limit.set_rpy(true);
214 140 : limit.throttle_lower = true;
215 140 : limit.throttle_upper = false;
216 140 : }
217 :
218 : // output - sends commands to the servos
219 3233979 : void AP_MotorsHeli::output()
220 : {
221 : // update throttle filter
222 3233979 : update_throttle_filter();
223 :
224 : // run spool logic
225 3233979 : output_logic();
226 :
227 3233979 : if (armed()) {
228 : // block servo_test from happening at disarm
229 2321602 : _servo_test_cycle_counter = 0;
230 2321602 : output_armed_stabilizing();
231 : } else {
232 912377 : output_disarmed();
233 : }
234 :
235 : // helicopters always run stabilizing flight controls
236 3233979 : move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
237 :
238 : // motor control must be updated after move_actuators because DDFP Tail Rotor output is based on move actuators()
239 3233979 : update_motor_control(_spool_desired);
240 :
241 3233979 : update_turbine_start();
242 :
243 3233979 : output_to_motors();
244 :
245 3233979 : };
246 :
247 : // sends commands to the motors
248 2321602 : void AP_MotorsHeli::output_armed_stabilizing()
249 : {
250 2321602 : calculate_armed_scalars();
251 :
252 : // if manual override active after arming, deactivate it and reinitialize servos
253 2321602 : if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) {
254 0 : reset_flight_controls();
255 : }
256 2321602 : }
257 :
258 : // output_disarmed - sends commands to the motors
259 912377 : void AP_MotorsHeli::output_disarmed()
260 : {
261 912377 : if (_servo_test_cycle_counter > 0){
262 : // set servo_test_flag
263 0 : _heliflags.servo_test_running = true;
264 : // perform boot-up servo test cycle if enabled
265 0 : servo_test();
266 : } else {
267 : // set servo_test flag
268 912377 : _heliflags.servo_test_running = false;
269 : // manual override (i.e. when setting up swash)
270 912377 : switch (_servo_mode) {
271 0 : case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
272 : // pass pilot commands straight through to swash
273 0 : _roll_in = _roll_radio_passthrough;
274 0 : _pitch_in = _pitch_radio_passthrough;
275 0 : _throttle_filter.reset(_throttle_radio_passthrough);
276 0 : _yaw_in = _yaw_radio_passthrough;
277 0 : break;
278 0 : case SERVO_CONTROL_MODE_MANUAL_CENTER:
279 : // fixate mid collective
280 0 : _roll_in = 0.0f;
281 0 : _pitch_in = 0.0f;
282 0 : _throttle_filter.reset(_collective_zero_thrust_pct);
283 0 : _yaw_in = 0.0f;
284 0 : break;
285 0 : case SERVO_CONTROL_MODE_MANUAL_MAX:
286 : // fixate max collective
287 0 : _roll_in = 0.0f;
288 0 : _pitch_in = 0.0f;
289 0 : _throttle_filter.reset(1.0f);
290 0 : if (_frame_class == MOTOR_FRAME_HELI_DUAL ||
291 0 : _frame_class == MOTOR_FRAME_HELI_QUAD) {
292 0 : _yaw_in = 0;
293 : } else {
294 0 : _yaw_in = 1;
295 : }
296 0 : break;
297 0 : case SERVO_CONTROL_MODE_MANUAL_MIN:
298 : // fixate min collective
299 0 : _roll_in = 0.0f;
300 0 : _pitch_in = 0.0f;
301 0 : _throttle_filter.reset(0.0f);
302 0 : if (_frame_class == MOTOR_FRAME_HELI_DUAL ||
303 0 : _frame_class == MOTOR_FRAME_HELI_QUAD) {
304 0 : _yaw_in = 0;
305 : } else {
306 0 : _yaw_in = -1;
307 : }
308 0 : break;
309 0 : case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
310 : // use servo_test function from child classes
311 0 : servo_test();
312 0 : break;
313 912377 : default:
314 : // no manual override
315 912377 : break;
316 : }
317 : }
318 :
319 : // ensure swash servo endpoints haven't been moved
320 912377 : init_outputs();
321 :
322 : // continuously recalculate scalars to allow setup
323 912377 : calculate_scalars();
324 912377 : }
325 :
326 : // set_desired_spool_state - set desired spool state with safety constraints
327 3194280 : void AP_MotorsHeli::set_desired_spool_state(DesiredSpoolState desired)
328 : {
329 : // Safety constraint: disarmed vehicles must be shut down
330 3194280 : if (!armed()) {
331 895860 : _spool_desired = DesiredSpoolState::SHUT_DOWN;
332 895860 : return;
333 : }
334 :
335 : // Safety constraint: without interlock, limit to GROUND_IDLE
336 : // (allows swash movement for autorotation, but rotor cannot spin up)
337 2298420 : if (!get_interlock()) {
338 153780 : switch (desired) {
339 0 : case DesiredSpoolState::SHUT_DOWN:
340 0 : _spool_desired = DesiredSpoolState::SHUT_DOWN;
341 0 : break;
342 153780 : case DesiredSpoolState::GROUND_IDLE:
343 : case DesiredSpoolState::THROTTLE_UNLIMITED:
344 153780 : _spool_desired = DesiredSpoolState::GROUND_IDLE;
345 153780 : break;
346 : }
347 153780 : return;
348 : }
349 :
350 : // No safety constraints active - accept requested state
351 2144640 : _spool_desired = desired;
352 : }
353 :
354 : // run spool logic
355 3233979 : void AP_MotorsHeli::output_logic()
356 : {
357 : // force desired and current spool mode if disarmed and armed with interlock enabled
358 3233979 : if (armed()) {
359 2321602 : if (!get_interlock()) {
360 165505 : _spool_desired = DesiredSpoolState::GROUND_IDLE;
361 : } else {
362 2156097 : _heliflags.init_targets_on_arming = false;
363 : }
364 : } else {
365 912377 : _heliflags.init_targets_on_arming = true;
366 912377 : _spool_desired = DesiredSpoolState::SHUT_DOWN; // RSC will enforce _spool_state to SHUT_DOWN when spool_desired is SHUT_DOWN
367 : }
368 :
369 : // send desired spool state update to Heli RSC and update outputs
370 : // the Heli RSC will return the current spool state which is used to update _spool_state variable
371 : // _spool_state is enforced to SHUT_DOWN when _spool_desired is SHUT_DOWN.
372 3233979 : _spool_state = update_spool_state(_spool_desired);
373 :
374 : // Always reset the collective limit flags, they get set in move_actuators() if collective reaches a limit
375 3233979 : limit.set_throttle(false);
376 :
377 3233979 : switch (_spool_state) {
378 912377 : case SpoolState::SHUT_DOWN:
379 : // Motors should be stationary.
380 : // Servos set to their trim values or in a test condition.
381 :
382 : // set limits flags
383 912377 : if (!using_leaky_integrator()) {
384 0 : limit.set_rpy(true);
385 : } else {
386 912377 : limit.set_rpy(false);
387 : }
388 912377 : break;
389 :
390 97503 : case SpoolState::GROUND_IDLE: {
391 : // Motors should be stationary or at ground idle.
392 : // set limits flags
393 97503 : if (_heliflags.land_complete && !using_leaky_integrator()) {
394 0 : limit.set_rpy(true);
395 : } else {
396 97503 : limit.set_rpy(false);
397 : }
398 97503 : break;
399 : }
400 158332 : case SpoolState::SPOOLING_UP:
401 : // Maximum throttle should move from minimum to maximum.
402 : // Servos should exhibit normal flight behavior.
403 :
404 : // set limits flags
405 158332 : if (_heliflags.land_complete && !using_leaky_integrator()) {
406 0 : limit.set_rpy(true);
407 : } else {
408 158332 : limit.set_rpy(false);
409 : }
410 :
411 158332 : break;
412 :
413 1995604 : case SpoolState::THROTTLE_UNLIMITED:
414 : // Throttle should exhibit normal flight behavior.
415 : // Servos should exhibit normal flight behavior.
416 :
417 : // set limits flags
418 1995604 : if (_heliflags.land_complete && !using_leaky_integrator()) {
419 0 : limit.set_rpy(true);
420 : } else {
421 1995604 : limit.set_rpy(false);
422 : }
423 :
424 1995604 : break;
425 :
426 70163 : case SpoolState::SPOOLING_DOWN:
427 : // Maximum throttle should move from maximum to minimum.
428 : // Servos should exhibit normal flight behavior.
429 :
430 : // set limits flags
431 70163 : if (_heliflags.land_complete && !using_leaky_integrator()) {
432 0 : limit.set_rpy(true);
433 : } else {
434 70163 : limit.set_rpy(false);
435 : }
436 70163 : break;
437 : }
438 :
439 3233979 : }
440 :
441 : // update the throttle input filter
442 3233979 : void AP_MotorsHeli::update_throttle_filter()
443 : {
444 3233979 : _throttle_filter.apply(_throttle_in, _dt_s);
445 :
446 : // constrain filtered throttle
447 3233979 : if (_throttle_filter.get() < 0.0f) {
448 1139235 : _throttle_filter.reset(0.0f);
449 : }
450 3233979 : if (_throttle_filter.get() > 1.0f) {
451 20309 : _throttle_filter.reset(1.0f);
452 : }
453 3233979 : }
454 :
455 : // reset_flight_controls - resets all controls and scalars to flight status
456 0 : void AP_MotorsHeli::reset_flight_controls()
457 : {
458 0 : _servo_mode.set(SERVO_CONTROL_MODE_AUTOMATED);
459 0 : init_outputs();
460 0 : calculate_scalars();
461 0 : }
462 :
463 : // update the collective input filter. should be called at 100hz
464 183677 : void AP_MotorsHeli::update_throttle_hover(float dt)
465 : {
466 183677 : if (_collective_hover_learn != HOVER_LEARN_DISABLED) {
467 :
468 : // Don't let _collective_hover go below H_COLL_ZERO_THRST
469 183677 : float curr_collective = get_throttle();
470 183677 : if (curr_collective < _collective_zero_thrust_pct) {
471 0 : curr_collective = _collective_zero_thrust_pct;
472 : }
473 :
474 : // we have chosen to constrain the hover collective to be within the range reachable by the third order expo polynomial.
475 183677 : _collective_hover.set(constrain_float(_collective_hover + (dt / (dt + AP_MOTORS_HELI_COLLECTIVE_HOVER_TC)) * (curr_collective - _collective_hover), AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX));
476 : }
477 183677 : }
478 :
479 : // save parameters as part of disarming
480 58 : void AP_MotorsHeli::save_params_on_disarm()
481 : {
482 : // save hover throttle
483 58 : if (_collective_hover_learn == HOVER_LEARN_AND_SAVE) {
484 58 : _collective_hover.save();
485 : }
486 58 : }
487 :
488 : // updates the takeoff collective flag
489 3234119 : void AP_MotorsHeli::update_takeoff_collective_flag(float coll_out)
490 : {
491 3234119 : if (coll_out > _collective_zero_thrust_pct + 0.5f * (_collective_hover - _collective_zero_thrust_pct)) {
492 1739016 : _heliflags.takeoff_collective = true;
493 : } else {
494 1495103 : _heliflags.takeoff_collective = false;
495 : }
496 3234119 : }
497 :
498 : // Determines if _heli_options bit is set
499 4258859 : bool AP_MotorsHeli::heli_option(HeliOption opt) const
500 : {
501 4258859 : return (_heli_options & (uint8_t)opt);
502 : }
503 :
504 : // updates the turbine start flag
505 3233979 : void AP_MotorsHeli::update_turbine_start()
506 : {
507 3233979 : if (_heliflags.start_engine) {
508 11920 : _main_rotor.set_turbine_start(true);
509 : } else {
510 3222059 : _main_rotor.set_turbine_start(false);
511 : }
512 3233979 : }
513 :
514 : // Run arming checks
515 2339 : bool AP_MotorsHeli::arming_checks(size_t buflen, char *buffer) const
516 : {
517 : // run base class checks
518 2339 : if (!AP_Motors::arming_checks(buflen, buffer)) {
519 0 : return false;
520 : }
521 :
522 2339 : if (_heliflags.servo_test_running) {
523 0 : hal.util->snprintf(buffer, buflen, "Servo Test is still running");
524 0 : return false;
525 : }
526 :
527 : // returns false if RSC Mode is not set to a valid control mode
528 2339 : if (_main_rotor._rsc_mode.get() <= (int8_t)ROTOR_CONTROL_MODE_DISABLED || _main_rotor._rsc_mode.get() > (int8_t)ROTOR_CONTROL_MODE_AUTOTHROTTLE) {
529 0 : hal.util->snprintf(buffer, buflen, "H_RSC_MODE invalid");
530 0 : return false;
531 : }
532 :
533 : // returns false if rsc_setpoint is out of range
534 2339 : if ( _main_rotor._rsc_setpoint.get() > 100 || _main_rotor._rsc_setpoint.get() < 10){
535 0 : hal.util->snprintf(buffer, buflen, "H_RSC_SETPOINT out of range");
536 0 : return false;
537 : }
538 :
539 : // returns false if idle output is out of range
540 2339 : if ( _main_rotor._idle_output.get() > 100 || _main_rotor._idle_output.get() < 0){
541 0 : hal.util->snprintf(buffer, buflen, "H_RSC_IDLE out of range");
542 0 : return false;
543 : }
544 :
545 : // returns false if _rsc_critical is not between 0 and 100
546 2339 : if (_main_rotor._critical_speed.get() > 100 || _main_rotor._critical_speed.get() < 0) {
547 0 : hal.util->snprintf(buffer, buflen, "H_RSC_CRITICAL out of range");
548 0 : return false;
549 : }
550 :
551 : // returns false if RSC Runup Time is less than Ramp time as this could cause undesired behaviour of rotor speed estimate
552 2339 : if (_main_rotor._runup_time.get() <= _main_rotor._ramp_time.get()){
553 0 : hal.util->snprintf(buffer, buflen, "H_RUNUP_TIME too small");
554 0 : return false;
555 : }
556 :
557 : // returns false if _collective_min_deg is not default value which indicates users set parameter
558 2339 : if (is_equal((float)_collective_min_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MIN_DEG)) {
559 0 : hal.util->snprintf(buffer, buflen, "Set H_COL_ANG_MIN to measured min blade pitch in deg");
560 0 : return false;
561 : }
562 :
563 : // returns false if _collective_max_deg is not default value which indicates users set parameter
564 2339 : if (is_equal((float)_collective_max_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MAX_DEG)) {
565 0 : hal.util->snprintf(buffer, buflen, "Set H_COL_ANG_MAX to measured max blade pitch in deg");
566 0 : return false;
567 : }
568 :
569 2339 : if (!_main_rotor.autorotation.arming_checks(buflen, buffer)) {
570 0 : return false;
571 : }
572 :
573 2339 : return true;
574 : }
575 :
576 : // Tell user motor test is disabled on heli
577 0 : bool AP_MotorsHeli::motor_test_checks(size_t buflen, char *buffer) const
578 : {
579 0 : hal.util->snprintf(buffer, buflen, "Disabled on heli");
580 0 : return false;
581 : }
582 :
583 : // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
584 : // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
585 3 : uint32_t AP_MotorsHeli::get_motor_mask()
586 : {
587 3 : return _main_rotor.get_output_mask();
588 : }
589 :
590 : // set_desired_rotor_speed
591 0 : void AP_MotorsHeli::set_desired_rotor_speed(float desired_rotor_speed)
592 : {
593 0 : _main_rotor.set_passthru_desired_rotor_speed(desired_rotor_speed);
594 0 : }
595 :
596 : // Update _heliflags.rotor_runup_complete value writing log event on state change
597 3234119 : void AP_MotorsHeli::set_rotor_runup_complete(bool new_value)
598 : {
599 : #if HAL_LOGGING_ENABLED
600 3234119 : if (!_heliflags.rotor_runup_complete && new_value) {
601 33 : LOGGER_WRITE_EVENT(LogEvent::ROTOR_RUNUP_COMPLETE);
602 3234086 : } else if (_heliflags.rotor_runup_complete && !new_value && !_main_rotor.in_autorotation()) {
603 33 : LOGGER_WRITE_EVENT(LogEvent::ROTOR_SPEED_BELOW_CRITICAL);
604 : }
605 : #endif
606 3234119 : _heliflags.rotor_runup_complete = new_value;
607 3234119 : }
608 :
609 : #if HAL_LOGGING_ENABLED
610 : // Returns the scaling value required to convert the collective angle parameters into the cyclic-output-to-angle conversion
611 76247 : float AP_MotorsHeli::get_cyclic_angle_scaler(void) const {
612 : // We want to use the collective min-max to angle relationship to calculate the cyclic input to angle relationship
613 : // First we scale the collective angle range by it's min-max output. Recall that we assume that the maximum possible
614 : // collective range is 1000, hence the *1e-3.
615 : // The factor 2.0 accounts for the fact that we scale the servo outputs from 0 ~ 1 to -1 ~ 1
616 76247 : return ((float)(_collective_max-_collective_min))*1e-3 * (_collective_max_deg.get() - _collective_min_deg.get()) * 2.0;
617 : }
618 : #endif
619 :
620 : // Helper function for param conversions to be done in motors class
621 83 : void AP_MotorsHeli::heli_motors_param_conversions(void)
622 : {
623 : // PARAMETER_CONVERSION - Added: Sep-2024
624 : // move autorotation related parameters within the RSC into their own class
625 83 : const AP_Param::ConversionInfo rsc_arot_conversion_info[] = {
626 : { 90, 108096, AP_PARAM_INT8, "H_RSC_AROT_ENBL" },
627 : { 90, 104000, AP_PARAM_INT8, "H_RSC_AROT_RAMP" },
628 : { 90, 112192, AP_PARAM_INT16, "H_RSC_AROT_IDLE" },
629 : };
630 83 : uint8_t table_size = ARRAY_SIZE(rsc_arot_conversion_info);
631 332 : for (uint8_t i=0; i<table_size; i++) {
632 249 : AP_Param::convert_old_parameter(&rsc_arot_conversion_info[i], 1.0);
633 : }
634 83 : }
635 :
636 : // function to calculate and set the normalised collective position given a desired blade pitch angle (deg)
637 0 : void AP_MotorsHeli::set_coll_from_ang(float col_ang_deg)
638 : {
639 0 : const float col_norm = (col_ang_deg - _collective_min_deg.get()) / MAX((_collective_max_deg.get() - _collective_min_deg.get()), 1.0);
640 0 : set_throttle(constrain_float(col_norm, 0.0, 1.0));
641 0 : }
|
