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 <GCS_MAVLink/GCS.h>
19 : #include "AP_MotorsHeli_RSC.h"
20 : #include <AP_RPM/AP_RPM.h>
21 : #include <AP_Logger/AP_Logger.h>
22 :
23 : // default main rotor speed (ch8 out) as a number from 0 ~ 100
24 : #define AP_MOTORS_HELI_RSC_SETPOINT 70
25 :
26 : // default main rotor critical speed
27 : #define AP_MOTORS_HELI_RSC_CRITICAL 50
28 :
29 : // RSC output defaults
30 : #define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0
31 :
32 : // default main rotor ramp up time in seconds
33 : #define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to setpoint
34 : #define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed
35 :
36 : // Throttle Curve Defaults
37 : #define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 25
38 : #define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 32
39 : #define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 38
40 : #define AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT 50
41 : #define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 100
42 :
43 : // RSC governor defaults
44 : #define AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT 100
45 :
46 :
47 : extern const AP_HAL::HAL& hal;
48 :
49 : const AP_Param::GroupInfo AP_MotorsHeli_RSC::var_info[] = {
50 :
51 : // @Param: SETPOINT
52 : // @DisplayName: External Motor Governor Setpoint
53 : // @Description: Throttle (HeliRSC Servo) output in percent to the external motor governor when motor interlock enabled (throttle hold off).
54 : // @Range: 0 100
55 : // @Units: %
56 : // @Increment: 1
57 : // @User: Standard
58 : AP_GROUPINFO("SETPOINT", 1, AP_MotorsHeli_RSC, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT),
59 :
60 : // @Param: MODE
61 : // @DisplayName: Rotor Speed Control Mode
62 : // @Description: Selects the type of rotor speed control used to determine throttle output to the HeliRSC servo channel when motor interlock is enabled (throttle hold off). RC Passthrough sends the input from the RC Motor Interlock channel as throttle output. External Gov SetPoint sends the RSC SetPoint parameter value as throttle output. Throttle Curve uses the 5 point throttle curve to determine throttle output based on the collective output. AutoThrottle requires a rotor speed sensor, contains an advanced autothrottle governor and is primarily for piston and turbine engines. WARNING: Throttle ramp time and throttle curve MUST be tuned properly using Throttle Curve mode before using AutoThrottle
63 : // @Values: 1:RC Passthrough, 2:External Gov SetPoint, 3:Throttle Curve, 4:AutoThrottle, 5:Direct Drive Fixed Pitch (Tail RSC only)
64 : // @User: Standard
65 : AP_GROUPINFO("MODE", 2, AP_MotorsHeli_RSC, _rsc_mode, (int8_t)ROTOR_CONTROL_MODE_PASSTHROUGH),
66 :
67 : // @Param: RAMP_TIME
68 : // @DisplayName: Throttle Ramp Time
69 : // @Description: Time in seconds for throttle output (HeliRSC servo) to ramp from ground idle (RSC_IDLE) to flight idle throttle setting when motor interlock is enabled (throttle hold off).
70 : // @Range: 0 60
71 : // @Units: s
72 : // @User: Standard
73 : AP_GROUPINFO("RAMP_TIME", 3, AP_MotorsHeli_RSC, _ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME),
74 :
75 : // @Param: RUNUP_TIME
76 : // @DisplayName: Rotor Runup Time
77 : // @Description: Actual time in seconds for the main rotor to reach full speed after motor interlock is enabled (throttle hold off). Must be at least one second longer than the Throttle Ramp Time that is set with RSC_RAMP_TIME. WARNING: For AutoThrottle users with piston and turbine engines it is VERY important to know how long it takes to warm up your engine and reach full rotor speed when throttle switch is turned ON. This timer should be set for at least the amount of time it takes to get your helicopter to full flight power, ready for takeoff. Failure to heed this warning could result in the auto-takeoff mode attempting to lift up into hover before the engine has reached full power, and subsequent loss of control
78 : // @Range: 0 60
79 : // @Units: s
80 : // @User: Standard
81 : AP_GROUPINFO("RUNUP_TIME", 4, AP_MotorsHeli_RSC, _runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME),
82 :
83 : // @Param: CRITICAL
84 : // @DisplayName: Critical Rotor Speed
85 : // @Description: Percentage of normal rotor speed where flight is no longer possible. However currently the rotor runup/rundown is estimated using the RSC_RUNUP_TIME parameter. Estimated rotor speed increases/decreases between 0 (rotor stopped) to 1 (rotor at normal speed) in the RSC_RUNUP_TIME in seconds. This parameter should be set so that the estimated rotor speed goes below critical in approximately 3 seconds. So if you had a 10 second runup time then set RSC_CRITICAL to 70%.
86 : // @Range: 0 100
87 : // @Units: %
88 : // @Increment: 1
89 : // @User: Standard
90 : AP_GROUPINFO("CRITICAL", 5, AP_MotorsHeli_RSC, _critical_speed, AP_MOTORS_HELI_RSC_CRITICAL),
91 :
92 : // @Param: IDLE
93 : // @DisplayName: Throttle Output at Idle
94 : // @Description: Throttle output (HeliRSC Servo) in percent while armed but motor interlock is disabled (throttle hold on). FOR COMBUSTION ENGINES. Sets the engine ground idle throttle percentage with clutch disengaged. This must be set to zero for electric helicopters under most situations. If the ESC has an autorotation window this can be set to keep the autorotation window open in the ESC. Consult the operating manual for your ESC to set it properly for this purpose
95 : // @Range: 0 50
96 : // @Units: %
97 : // @Increment: 1
98 : // @User: Standard
99 : AP_GROUPINFO("IDLE", 6, AP_MotorsHeli_RSC, _idle_output, AP_MOTORS_HELI_RSC_IDLE_DEFAULT),
100 :
101 : // @Param: SLEWRATE
102 : // @DisplayName: Throttle Slew Rate
103 : // @Description: This controls the maximum rate at which the throttle output (HeliRSC servo) can change, as a percentage per second. A value of 100 means the throttle can change over its full range in one second. A value of zero gives unlimited slew rate.
104 : // @Range: 0 500
105 : // @Increment: 10
106 : // @User: Standard
107 : AP_GROUPINFO("SLEWRATE", 7, AP_MotorsHeli_RSC, _power_slewrate, 0),
108 :
109 : // @Param: THRCRV_0
110 : // @DisplayName: Throttle Curve at 0% Coll
111 : // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at the minimum collective pitch position. The 0 percent collective is defined by H_COL_MIN. Example: if the setup has -2 degree to +10 degree collective pitch setup, this setting would correspond to -2 degree of pitch.
112 : // @Range: 0 100
113 : // @Units: %
114 : // @Increment: 1
115 : // @User: Standard
116 : AP_GROUPINFO("THRCRV_0", 8, AP_MotorsHeli_RSC, _thrcrv[0], AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT),
117 :
118 : // @Param: THRCRV_25
119 : // @DisplayName: Throttle Curve at 25% Coll
120 : // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at 25% of full collective travel where he 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. Example: if the setup has -2 degree to +10 degree collective pitch setup, the total range is 12 degrees. 25% of 12 degrees is 3 degrees, so this setting would correspond to +1 degree of pitch.
121 : // @Range: 0 100
122 : // @Units: %
123 : // @Increment: 1
124 : // @User: Standard
125 : AP_GROUPINFO("THRCRV_25", 9, AP_MotorsHeli_RSC, _thrcrv[1], AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT),
126 :
127 : // @Param: THRCRV_50
128 : // @DisplayName: Throttle Curve at 50% Coll
129 : // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at 50% of full collective travel where he 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. Example: if the setup has -2 degree to +10 degree collective pitch setup, the total range is 12 degrees. 50% of 12 degrees is 6 degrees, so this setting would correspond to +4 degree of pitch.
130 : // @Range: 0 100
131 : // @Units: %
132 : // @Increment: 1
133 : // @User: Standard
134 : AP_GROUPINFO("THRCRV_50", 10, AP_MotorsHeli_RSC, _thrcrv[2], AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT),
135 :
136 : // @Param: THRCRV_75
137 : // @DisplayName: Throttle Curve at 75% Coll
138 : // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at 75% of full collective travel where he 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. Example: if the setup has -2 degree to +10 degree collective pitch setup, the total range is 12 degrees. 75% of 12 degrees is 9 degrees, so this setting would correspond to +7 degree of pitch.
139 : // @Range: 0 100
140 : // @Units: %
141 : // @Increment: 1
142 : // @User: Standard
143 : AP_GROUPINFO("THRCRV_75", 11, AP_MotorsHeli_RSC, _thrcrv[3], AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT),
144 :
145 : // @Param: THRCRV_100
146 : // @DisplayName: Throttle Curve at 100% Coll
147 : // @Description: Sets the throttle output (HeliRSC servo) in percent for the throttle curve at the minimum collective pitch position. The 100 percent collective is defined by H_COL_MAX. Example: if the setup has -2 degree to +10 degree collective pitch setup, this setting would correspond to +10 degree of pitch.
148 : // @Range: 0 100
149 : // @Units: %
150 : // @Increment: 1
151 : // @User: Standard
152 : AP_GROUPINFO("THRCRV_100", 12, AP_MotorsHeli_RSC, _thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT),
153 :
154 : // Indices 13 thru 16 have been re-assigned and should not be used in the future
155 :
156 : // @Param: GOV_RANGE
157 : // @DisplayName: Governor Operational Range
158 : // @Description: RPM range +/- governor rpm reference setting where governor is operational. If speed sensor fails or rpm falls outside of this range, the governor will disengage and return to throttle curve. Recommended range is 100
159 : // @Range: 50 200
160 : // @Units: RPM
161 : // @Increment: 10
162 : // @User: Standard
163 : AP_GROUPINFO("GOV_RANGE", 17, AP_MotorsHeli_RSC, _governor_range, AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT),
164 :
165 : // Index 18 was renamed from AROT_PCT to AROT_IDLE
166 :
167 : // @Param: CLDWN_TIME
168 : // @DisplayName: Cooldown Time
169 : // @Description: Will provide a fast idle for engine cooldown by raising the Ground Idle speed setting by 50% for the number of seconds the timer is set for. A setting of zero disables the fast idle. This feature will only apply after the runup complete has been declared. This will not extend the time before ground idle is declared, which triggers engine shutdown for autonomous landings.
170 : // @Range: 0 120
171 : // @Units: s
172 : // @Increment: 1
173 : // @User: Standard
174 : AP_GROUPINFO("CLDWN_TIME", 19, AP_MotorsHeli_RSC, _cooldown_time, 0),
175 :
176 : // @Param: GOV_COMP
177 : // @DisplayName: Governor Torque Compensator
178 : // @Description: Adjusts the autothrottle governor torque compensator that determines how fast the governor will adjust the base torque reference to compensate for changes in density altitude. If RPM is low or high by more than 2-5 RPM, increase this setting by 1% at a time until the governor speed matches your RPM setting. Setting the compensator too high can result in surging and throttle "hunting". Do not make large adjustments at one time
179 : // @Range: 0 70
180 : // @Units: %
181 : // @Increment: 0.1
182 : // @User: Standard
183 : AP_GROUPINFO("GOV_COMP", 20, AP_MotorsHeli_RSC, _governor_compensator, 25),
184 :
185 : // @Param: GOV_DROOP
186 : // @DisplayName: Governor Droop Compensator
187 : // @Description: AutoThrottle governor droop response under load, normal settings of 0-50%. Higher value is quicker response to large speed changes due to load but may cause surging. Adjust this to be as aggressive as possible without getting surging or RPM over-run when the governor responds to large load changes on the rotor system
188 : // @Range: 0 100
189 : // @Units: %
190 : // @Increment: 0.1
191 : // @User: Standard
192 : AP_GROUPINFO("GOV_DROOP", 21, AP_MotorsHeli_RSC, _governor_droop_response, 25),
193 :
194 : // @Param: GOV_FF
195 : // @DisplayName: Governor Feedforward
196 : // @Description: Feedforward governor gain to throttle response during sudden loading/unloading of the rotor system. If RPM drops excessively during full collective climb with the droop response set correctly, increase the governor feedforward.
197 : // @Range: 0 100
198 : // @Units: %
199 : // @Increment: 0.1
200 : // @User: Standard
201 : AP_GROUPINFO("GOV_FF", 22, AP_MotorsHeli_RSC, _governor_ff, 50),
202 :
203 : // @Param: GOV_RPM
204 : // @DisplayName: Rotor RPM Setting
205 : // @Description: Main rotor RPM that governor maintains when engaged
206 : // @Range: 800 3500
207 : // @Units: RPM
208 : // @Increment: 10
209 : // @User: Standard
210 : AP_GROUPINFO("GOV_RPM", 23, AP_MotorsHeli_RSC, _governor_rpm, 1500),
211 :
212 : // @Param: GOV_TORQUE
213 : // @DisplayName: Governor Torque Limiter
214 : // @Description: Adjusts the engine's percentage of torque rise on autothrottle during ramp-up to governor speed. The torque rise will determine how fast the rotor speed will ramp up when rotor speed reaches 50% of the rotor RPM setting. The sequence of events engaging the governor is as follows: Throttle ramp time will engage the clutch and start the main rotor turning. The collective should be at flat pitch and the throttle curve set to provide at least 50% of normal RPM at flat pitch. The autothrottle torque limiter will automatically activate and start accelerating the main rotor. If the autothrottle consistently fails to accelerate the main rotor during ramp-in due to engine tune or other factors, then increase the torque limiter setting. NOTE: throttle ramp time and throttle curve should be tuned using RSC_MODE Throttle Curve before using RSC_MODE AutoThrottle
215 : // @Range: 10 60
216 : // @Units: %
217 : // @Increment: 1
218 : // @User: Standard
219 : AP_GROUPINFO("GOV_TORQUE", 24, AP_MotorsHeli_RSC, _governor_torque, 30),
220 :
221 : // 25 was AROT_ENG_T, has been moved to AROT_RAMP in RSC autorotation sub group
222 :
223 : // 26 was AROT_MN_EN, moved to H_RSC_AROT_ENBL in RSC autorotation sub group
224 :
225 : // 27 was AROT_IDLE, moved to RSC autorotation sub group
226 :
227 : AP_GROUPEND
228 : };
229 :
230 : // init_servo - servo initialization on start-up
231 86 : void AP_MotorsHeli_RSC::initialize()
232 : {
233 : // setup RSC on specified channel by default
234 86 : SRV_Channels::set_aux_channel_default(_aux_fn, _default_channel);
235 :
236 : // configure RSC on initialization
237 86 : configure();
238 :
239 86 : }
240 :
241 : // configure - configure the RSC using current parameters
242 912546 : void AP_MotorsHeli_RSC::configure()
243 : {
244 :
245 912546 : configure((RotorControlMode)(_rsc_mode.get()), _ramp_time.get(), _runup_time.get(), _critical_speed.get(), _idle_output.get());
246 :
247 912546 : }
248 :
249 : // configure - configure the RSC with specific settings, allows caller to specify settings instead of using parameters.
250 1772391 : void AP_MotorsHeli_RSC::configure(RotorControlMode control_mode, int8_t ramp_time, int8_t runup_time, float critical_speed, float idle_output)
251 : {
252 1772391 : set_rsc_control_mode(control_mode);
253 1772391 : set_ramp_time(ramp_time);
254 1772391 : set_runup_time(runup_time);
255 1772391 : set_critical_speed(critical_speed);
256 1772391 : set_idle_output(idle_output);
257 :
258 : // set desired rotor speed for setpoint mode from parameter.
259 1772391 : if (_rsc_control_mode == ROTOR_CONTROL_MODE_SETPOINT) {
260 893330 : _setpoint_desired_rotor_speed = _rsc_setpoint.get() * 0.01f;
261 : }
262 :
263 1772391 : }
264 :
265 : // configure - configure the RSC.
266 3432968 : void AP_MotorsHeli_RSC::configure_armed()
267 : {
268 :
269 : // set desired speed for each control mode
270 3432968 : switch (_rsc_control_mode) {
271 0 : case ROTOR_CONTROL_MODE_PASSTHROUGH:
272 : // passthrough mode uses the pilot's desired speed directly as the control output, so set the desired speed to the pilot's input
273 0 : _desired_rotor_speed = _passthru_desired_rotor_speed;
274 0 : break;
275 3173287 : case ROTOR_CONTROL_MODE_SETPOINT:
276 : // allows setpoint to set from parameter but also allows it to be updated by caller
277 3173287 : _desired_rotor_speed = _setpoint_desired_rotor_speed;
278 3173287 : break;
279 151000 : case ROTOR_CONTROL_MODE_DDFP:
280 151000 : _desired_rotor_speed = _ddfp_desired_rotor_speed;
281 151000 : break;
282 108681 : case ROTOR_CONTROL_MODE_THROTTLECURVE:
283 : case ROTOR_CONTROL_MODE_AUTOTHROTTLE:
284 : // throttle curve and autothrottle both use the pilot's desired speed as the input to the throttle curve, so set the desired speed to the pilot's input
285 108681 : _desired_rotor_speed = 1.0f;
286 108681 : break;
287 0 : case ROTOR_CONTROL_MODE_DISABLED:
288 : // in disabled mode the desired speed is not used, but set it to zero for safety
289 0 : _desired_rotor_speed = 0.0f;
290 0 : break;
291 : }
292 :
293 : // Set rsc mode specific parameters
294 3432968 : if (_rsc_mode.get() == ROTOR_CONTROL_MODE_THROTTLECURVE || _rsc_mode.get() == ROTOR_CONTROL_MODE_AUTOTHROTTLE) {
295 108681 : set_throttle_curve();
296 : }
297 :
298 : // keeps user from changing RSC mode while armed
299 3432968 : if (_rsc_mode.get() != get_rsc_control_mode()) {
300 3 : reset_rsc_mode_param();
301 3 : _save_rsc_mode = true;
302 : }
303 :
304 : // saves rsc mode parameter when disarmed if it had been reset while armed
305 3432968 : if (_save_rsc_mode && _desired_spool_state == DesiredRSCSpoolState::SHUT_DOWN) {
306 3 : _rsc_mode.save();
307 3 : _save_rsc_mode = false;
308 : }
309 :
310 3432968 : }
311 :
312 : // set_power_output_range
313 : // TODO: Look at possibly calling this at a slower rate. Doesn't need to be called every cycle.
314 108681 : void AP_MotorsHeli_RSC::set_throttle_curve()
315 : {
316 : float thrcrv[5];
317 : // Ensure user inputs are within parameter limits
318 : // Scale throttle curve parameters
319 652086 : for (uint8_t i = 0; i < 5; i++) {
320 543405 : thrcrv[i] = constrain_float(_thrcrv[i] * 0.01f, 0.0f, 1.0f);
321 : }
322 : // Calculate the spline polynomials for the throttle curve
323 108681 : splinterp5(thrcrv,_thrcrv_poly);
324 108681 : }
325 :
326 : // update - ran each loop to update the RSC
327 6286516 : void AP_MotorsHeli_RSC::update(float dt)
328 : {
329 :
330 : // if control mode is disabled, then control output is forced to zero and no other updates are needed
331 6286516 : if (_rsc_control_mode == ROTOR_CONTROL_MODE_DISABLED) {
332 2853488 : _control_output = 0.0f;
333 2853488 : return;
334 : }
335 :
336 : // _rotor_RPM available to the RSC output
337 : #if AP_RPM_ENABLED
338 3433028 : const AP_RPM *rpm = AP_RPM::get_singleton();
339 3433028 : if (rpm != nullptr) {
340 3433028 : if (!rpm->get_rpm(0, _rotor_rpm)) {
341 : // No valid RPM data
342 89318 : _rotor_rpm = -1;
343 : }
344 : } else {
345 : // No RPM because pointer is null
346 0 : _rotor_rpm = -1;
347 : }
348 : #else
349 : _rotor_rpm = -1;
350 : #endif
351 :
352 3433028 : float last_control_output = _control_output;
353 :
354 3433028 : switch (_desired_spool_state) {
355 963997 : case DesiredRSCSpoolState::SHUT_DOWN:
356 :
357 : // control output forced to zero
358 963997 : _control_output = 0.0f;
359 :
360 : // governor is forced to disengage status and reset outputs
361 963997 : governor_reset();
362 963997 : _autothrottle = false;
363 963997 : _governor_fault = false;
364 : //turbine start flag on
365 963997 : _starting = true;
366 :
367 : // ensure we always deactivate the autorotation state if we disarm
368 963997 : autorotation.set_active(false, true);
369 :
370 : // ensure _idle_throttle not set to invalid value
371 963997 : _idle_throttle = get_idle_output();
372 :
373 : // reset fast idle timer
374 963997 : _fast_idle_timer = 0.0;
375 963997 : break;
376 :
377 256430 : case DesiredRSCSpoolState::GROUND_IDLE:
378 : // set rotor control speed to engine idle and ensure governor is reset, if used
379 256430 : governor_reset();
380 256430 : _autothrottle = false;
381 256430 : _governor_fault = false;
382 :
383 : // turbine start sequence
384 256430 : if (_turbine_start && _starting == true ) {
385 2053 : _idle_throttle += 0.001f;
386 2053 : if (_control_output >= 1.0f) {
387 2 : _idle_throttle = get_idle_output();
388 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Turbine startup");
389 2 : _starting = false;
390 : }
391 2053 : _control_output = _idle_throttle;
392 2053 : break;
393 : }
394 :
395 : // all other idle throttle functions below this require idle throttle to be reset to H_RSC_IDLE on each call
396 254377 : _idle_throttle = get_idle_output();
397 :
398 : // check if we need to use autorotation idle throttle
399 254377 : if (autorotation.get_idle_throttle(_idle_throttle)) {
400 : // if we are here then we are autorotating
401 27423 : _control_output = _idle_throttle;
402 27423 : break;
403 : }
404 :
405 : // check if we need to use engine cooldown
406 226954 : if (_fast_idle_timer > 0.0) {
407 : // running at fast idle for engine cool down
408 6003 : _idle_throttle *= 1.5;
409 6003 : _fast_idle_timer -= dt;
410 : }
411 :
412 226954 : _control_output = _idle_throttle;
413 :
414 226954 : break;
415 :
416 2212601 : case DesiredRSCSpoolState::THROTTLE_UNLIMITED:
417 : // set fast idle timer so next time RSC goes to idle, the cooldown timer starts
418 2212601 : if (_cooldown_time.get() > 0) {
419 69344 : _fast_idle_timer = _cooldown_time.get();
420 : }
421 :
422 : // ensure _idle_throttle not set to invalid value due to premature switch out of turbine start
423 2212601 : if (_starting) {
424 37 : _idle_throttle = get_idle_output();
425 : }
426 : // if turbine engine started without using start sequence, set starting flag just to be sure it can't be triggered when back in idle
427 2212601 : _starting = false;
428 :
429 2212601 : if ((_rsc_control_mode == ROTOR_CONTROL_MODE_PASSTHROUGH) || (_rsc_control_mode == ROTOR_CONTROL_MODE_SETPOINT) || (_rsc_control_mode == ROTOR_CONTROL_MODE_DDFP)) {
430 : // set control rotor speed to ramp slewed value between idle and desired speed
431 2154237 : _control_output = _idle_throttle + (_rotor_ramp_output * (_desired_rotor_speed - _idle_throttle));
432 58364 : } else if (_rsc_control_mode == ROTOR_CONTROL_MODE_THROTTLECURVE) {
433 : // throttle output from throttle curve based on collective position
434 0 : float throttlecurve = calculate_throttlecurve(_collective_in);
435 0 : _control_output = _idle_throttle + (_rotor_ramp_output * (throttlecurve - _idle_throttle));
436 58364 : } else if (_rsc_control_mode == ROTOR_CONTROL_MODE_AUTOTHROTTLE) {
437 58364 : autothrottle_run();
438 : }
439 2212601 : break;
440 : }
441 :
442 3433028 : if (_power_slewrate > 0) {
443 : // implement slew rate for throttle
444 0 : float max_delta = dt * _power_slewrate * 0.01f;
445 0 : _control_output = constrain_float(_control_output, last_control_output-max_delta, last_control_output+max_delta);
446 : }
447 : }
448 :
449 : // update_spool_state - updates the spool state machine based on the desired spool state and current spool state
450 6286516 : AP_MotorsHeli_RSC::RSCSpoolState AP_MotorsHeli_RSC::update_spool_state(AP_MotorsHeli_RSC::DesiredRSCSpoolState desired_spool_state, float dt)
451 : {
452 :
453 : // if control mode is disabled, then we should always be in SHUT_DOWN spool state and ignore any other desired spool state inputs
454 6286516 : if (_rsc_control_mode == ROTOR_CONTROL_MODE_DISABLED) {
455 2853488 : desired_spool_state = DesiredRSCSpoolState::SHUT_DOWN;
456 : }
457 :
458 : // set desired spool state
459 6286516 : _desired_spool_state = desired_spool_state;
460 :
461 6286516 : switch (_desired_spool_state) {
462 3817485 : case DesiredRSCSpoolState::SHUT_DOWN:
463 : // if we are shutting down, we want to immediately go to SHUT_DOWN state and not wait for spool down to complete
464 3817485 : _spool_state = RSCSpoolState::SHUT_DOWN;
465 : // set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
466 3817485 : update_rotor_ramp(0.0f, dt);
467 3817485 : break;
468 :
469 256430 : case DesiredRSCSpoolState::GROUND_IDLE:
470 : // set rotor ramp to decrease speed to zero
471 256430 : update_rotor_ramp(0.0f, dt);
472 256430 : break;
473 :
474 2212601 : case DesiredRSCSpoolState::THROTTLE_UNLIMITED:
475 : // set main rotor ramp to increase to full speed
476 2212601 : update_rotor_ramp(1.0f, dt);
477 2212601 : break;
478 : }
479 :
480 : // update rotor speed run-up estimate
481 6286516 : update_rotor_runup(dt);
482 :
483 6286516 : switch (_spool_state) {
484 3817546 : case RSCSpoolState::SHUT_DOWN:
485 : // Motors should be stationary.
486 : // make sure the motors are spooling in the correct direction
487 3817546 : if (_desired_spool_state != DesiredRSCSpoolState::SHUT_DOWN) {
488 61 : _spool_state = RSCSpoolState::GROUND_IDLE;
489 61 : break;
490 : }
491 :
492 3817485 : break;
493 :
494 99852 : case RSCSpoolState::GROUND_IDLE: {
495 : // Motors should be stationary or at ground idle.
496 99852 : if (_desired_spool_state == DesiredRSCSpoolState::SHUT_DOWN){
497 0 : _spool_state = RSCSpoolState::SHUT_DOWN;
498 99852 : } else if(_desired_spool_state == DesiredRSCSpoolState::THROTTLE_UNLIMITED) {
499 39 : _spool_state = RSCSpoolState::SPOOLING_UP;
500 : } else { // _desired_spool_state == GROUND_IDLE
501 :
502 : }
503 :
504 99852 : break;
505 : }
506 170329 : case RSCSpoolState::SPOOLING_UP:
507 : // Maximum throttle should move from minimum to maximum.
508 : // make sure the motors are spooling in the correct direction
509 170329 : if (_desired_spool_state != DesiredRSCSpoolState::THROTTLE_UNLIMITED ){
510 3 : _spool_state = RSCSpoolState::SPOOLING_DOWN;
511 3 : break;
512 : }
513 :
514 170326 : if (_runup_complete){
515 36 : _spool_state = RSCSpoolState::THROTTLE_UNLIMITED;
516 : }
517 170326 : break;
518 :
519 2126628 : case RSCSpoolState::THROTTLE_UNLIMITED:
520 : // Throttle should exhibit normal flight behavior.
521 : // make sure the motors are spooling in the correct direction
522 2126628 : if (_desired_spool_state != DesiredRSCSpoolState::THROTTLE_UNLIMITED && !rotor_speed_above_critical()) {
523 32 : _spool_state = RSCSpoolState::SPOOLING_DOWN;
524 32 : break;
525 : }
526 :
527 2126596 : break;
528 :
529 72161 : case RSCSpoolState::SPOOLING_DOWN:
530 : // make sure the motors are spooling in the correct direction
531 72161 : if (_desired_spool_state == DesiredRSCSpoolState::THROTTLE_UNLIMITED) {
532 0 : _spool_state = RSCSpoolState::SPOOLING_UP;
533 0 : break;
534 : }
535 72161 : if (_spooldown_complete){
536 33 : _spool_state = RSCSpoolState::GROUND_IDLE;
537 : }
538 72161 : break;
539 : }
540 6286516 : return _spool_state;
541 : }
542 :
543 : // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
544 6286516 : void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt)
545 : {
546 6286516 : float ramp_time = MAX(float(_ramp_time.get()), 1.0);
547 :
548 : // check if we need to use the bailout ramp up rate for the autorotation case
549 6286516 : if (autorotation.bailing_out()) {
550 9760 : ramp_time = autorotation.get_bailout_ramp();
551 : }
552 :
553 : // ramp output upwards towards target
554 6286516 : if (_rotor_ramp_output < rotor_ramp_input) {
555 44052 : _rotor_ramp_output += (dt / ramp_time);
556 :
557 : // Do not allow output to exceed requested input
558 44052 : _rotor_ramp_output = MIN(_rotor_ramp_output, rotor_ramp_input);
559 :
560 : } else {
561 : // ramping down happens instantly
562 6242464 : _rotor_ramp_output = rotor_ramp_input;
563 : }
564 6286516 : }
565 :
566 : // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_output
567 : // Relies on set_using_manual_collective_mode() having been called first. This method requires this information
568 : // to determine if the governor must be engaged for runup to be complete when using autothrottle RSC mode.
569 6286516 : void AP_MotorsHeli_RSC::update_rotor_runup(float dt)
570 : {
571 6286516 : float runup_time = _runup_time;
572 : // sanity check runup time
573 6286516 : runup_time = MAX(_ramp_time+1,runup_time);
574 :
575 : // adjust rotor runup when in autorotation or bailing out
576 6286516 : if (in_autorotation()) {
577 37183 : runup_time = autorotation.get_runup_time();
578 : }
579 :
580 : // protect against divide by zero
581 6286516 : runup_time = MAX(1,runup_time);
582 :
583 : // ramp speed estimate towards control out
584 6286516 : float runup_increment = dt / runup_time;
585 6286516 : if (_rotor_runup_output < _rotor_ramp_output) {
586 167329 : _rotor_runup_output += runup_increment;
587 167329 : if (_rotor_runup_output > _rotor_ramp_output) {
588 42 : _rotor_runup_output = _rotor_ramp_output;
589 : }
590 : } else {
591 6119187 : _rotor_runup_output -= runup_increment;
592 6119187 : if (_rotor_runup_output < _rotor_ramp_output) {
593 5954848 : _rotor_runup_output = _rotor_ramp_output;
594 : }
595 : }
596 : // if in autorotation, don't let rotor_runup_output go less than critical speed to keep
597 : // runup complete flag from being set to false
598 6286516 : if (in_autorotation() && !rotor_speed_above_critical()) {
599 27058 : _rotor_runup_output = get_critical_speed();
600 : }
601 :
602 : // update run-up complete flag
603 :
604 : // if control mode is disabled, then run-up complete always returns true
605 6286516 : if ( _rsc_control_mode == ROTOR_CONTROL_MODE_DISABLED ) {
606 2853488 : _runup_complete = true;
607 2853488 : return;
608 : }
609 :
610 : // rotor runup complete depends on the use of autothrottle RSC mode and the manual collective flight mode
611 : // if autothrottle is used and a non-manual collective mode is used, then the governor must be engaged for runup to be complete. Otherwise,
612 : // runup is complete when the rotor ramp and runup outputs are both at 1.0.
613 3433028 : if (!_runup_complete && (_rotor_ramp_output >= 1.0f) && (_rotor_runup_output >= 1.0f) &&
614 8032 : (_using_manual_collective_mode || _rsc_control_mode != ROTOR_CONTROL_MODE_AUTOTHROTTLE || _governor_engage)) {
615 : // warn user if runup timer completed but governor not engaged when using manual collective mode and autothrottle RSC mode
616 36 : if (_using_manual_collective_mode && _rsc_control_mode == ROTOR_CONTROL_MODE_AUTOTHROTTLE && _governor_engage == false) {
617 1 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Governor Failed to Engage when Runup Completed");
618 : } else {
619 35 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Runup Complete");
620 : }
621 36 : _runup_complete = true;
622 : }
623 : // if rotor speed is less than critical speed, then run-up is not complete
624 : // this will prevent the case where the target rotor speed is less than critical speed
625 3433028 : if (_runup_complete && !rotor_speed_above_critical()) {
626 32 : _runup_complete = false;
627 : }
628 : // if rotor estimated speed is zero, then spooldown has been completed
629 3433028 : if (_rotor_runup_output <= 0.0f) {
630 1057864 : _spooldown_complete = true;
631 : } else {
632 2375164 : _spooldown_complete = false;
633 : }
634 : }
635 :
636 : // write_rsc - outputs pwm onto output rsc channel
637 : // servo_out parameter is of the range 0 ~ 1
638 6286516 : void AP_MotorsHeli_RSC::write_rsc(float servo_out)
639 : {
640 6286516 : if (_rsc_control_mode == ROTOR_CONTROL_MODE_DISABLED) {
641 : // do not do servo output to avoid conflicting with other output on the channel
642 : // ToDo: We should probably use RC_Channel_Aux to avoid this problem
643 2853488 : return;
644 : } else {
645 3433028 : SRV_Channels::set_output_scaled(_aux_fn, servo_out * 1000);
646 : }
647 : }
648 :
649 : // Return mask of output channels which the RSC is outputting on
650 165 : uint32_t AP_MotorsHeli_RSC::get_output_mask() const
651 : {
652 165 : if (_rsc_control_mode == ROTOR_CONTROL_MODE_DISABLED) {
653 78 : return 0;
654 : }
655 87 : return SRV_Channels::get_output_channel_mask(_aux_fn);
656 : }
657 :
658 : // calculate_throttlecurve - uses throttle curve and collective input to determine throttle setting
659 58364 : float AP_MotorsHeli_RSC::calculate_throttlecurve(float collective_in)
660 : {
661 58364 : const float inpt = collective_in * 4.0f + 1.0f;
662 58364 : uint8_t idx = constrain_int16(int8_t(collective_in * 4), 0, 3);
663 58364 : const float a = inpt - (idx + 1.0f);
664 58364 : const float b = (idx + 1.0f) - inpt + 1.0f;
665 58364 : float throttle = _thrcrv_poly[idx][0] * a + _thrcrv_poly[idx][1] * b + _thrcrv_poly[idx][2] * (powf(a,3.0f) - a) / 6.0f + _thrcrv_poly[idx][3] * (powf(b,3.0f) - b) / 6.0f;
666 :
667 58364 : throttle = constrain_float(throttle, 0.0f, 1.0f);
668 58364 : return throttle;
669 :
670 : }
671 :
672 : // autothrottle_run - calculate throttle output for governor controlled throttle
673 58364 : void AP_MotorsHeli_RSC::autothrottle_run()
674 : {
675 58364 : float throttlecurve = calculate_throttlecurve(_collective_in);
676 58364 : float const torque_ref_error_rpm = 2.0f;
677 :
678 : // if the desired governor RPM is zero, use the throttle curve only and exit
679 58364 : if (_governor_rpm == 0) {
680 0 : _control_output = _idle_throttle + (_rotor_ramp_output * (throttlecurve - _idle_throttle));
681 0 : return;
682 : }
683 :
684 : // autothrottle main power loop with governor
685 58364 : if (_governor_engage && !_governor_fault) {
686 : // calculate droop - difference between actual and desired speed
687 28165 : float governor_droop = ((float)_governor_rpm - _rotor_rpm) * _governor_droop_response * 0.0001f;
688 28165 : _governor_output = governor_droop + ((throttlecurve - _governor_torque_reference) * _governor_ff * 0.01);
689 28165 : if (_rotor_rpm < ((float)_governor_rpm - torque_ref_error_rpm)) {
690 7374 : _governor_torque_reference += get_governor_compensator(); // torque compensator
691 20791 : } else if (_rotor_rpm > ((float)_governor_rpm + torque_ref_error_rpm)) {
692 5952 : _governor_torque_reference -= get_governor_compensator();
693 : }
694 : // throttle output uses droop + torque compensation to maintain proper rotor speed
695 28165 : _control_output = constrain_float((_governor_torque_reference + _governor_output), (get_idle_output() * 1.5f), 1.0f);
696 : // governor and speed sensor fault detection - must maintain RPM within governor range
697 : // speed fault detector will allow a fault to persist for 200 contiguous governor updates
698 : // this is failsafe for bad speed sensor or severely mis-adjusted governor
699 28165 : if ((_rotor_rpm <= (_governor_rpm - _governor_range)) || (_rotor_rpm >= (_governor_rpm + _governor_range))) {
700 0 : _governor_fault_count++;
701 0 : if (_governor_fault_count > 200) {
702 0 : governor_reset();
703 0 : _governor_fault = true;
704 0 : if (_rotor_rpm >= (_governor_rpm + _governor_range)) {
705 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed");
706 : } else {
707 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Underspeed");
708 : }
709 : }
710 : } else {
711 28165 : _governor_fault_count = 0; // reset fault count if the fault doesn't persist
712 : }
713 58364 : } else if (!_governor_engage && !_governor_fault) {
714 : // if governor is not engaged and rotor is overspeeding by more than governor range due to
715 : // misconfigured throttle curve or stuck throttle, set a fault and governor will not operate
716 30199 : if (_rotor_rpm > (_governor_rpm + _governor_range) && !autorotation.bailing_out()) {
717 0 : _governor_fault = true;
718 0 : governor_reset();
719 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed");
720 0 : _governor_output = 0.0f;
721 :
722 : // when performing power recovery from autorotation, this waits for user to load rotor in order to
723 : // engage the governor
724 30199 : } else if (_rotor_rpm > _governor_rpm && autorotation.bailing_out()) {
725 0 : _governor_output = 0.0f;
726 :
727 : // torque rise limiter accelerates rotor to the reference speed
728 : // this limits the max torque rise the governor could call for from the main power loop
729 30199 : } else if (_rotor_rpm >= (_governor_rpm * 0.5f)) {
730 1929 : float torque_limit = (get_governor_torque() * get_governor_torque());
731 1929 : _governor_output = (_rotor_rpm / (float)_governor_rpm) * torque_limit;
732 1929 : if (_rotor_rpm >= ((float)_governor_rpm - torque_ref_error_rpm)) {
733 1 : _governor_engage = true;
734 1 : _autothrottle = true;
735 : }
736 : } else {
737 : // temporary use of throttle curve and ramp timer to accelerate rotor to governor min torque rise speed
738 28270 : _governor_output = 0.0f;
739 : }
740 30199 : _control_output = constrain_float(_idle_throttle + (_rotor_ramp_output * (throttlecurve + _governor_output - _idle_throttle)), 0.0f, 1.0f);
741 30199 : _governor_torque_reference = _control_output; // increment torque setting to be passed to main power loop
742 30199 : } else {
743 : // failsafe - if governor has faulted use throttle curve
744 0 : _control_output = _idle_throttle + (_rotor_ramp_output * (throttlecurve - _idle_throttle));
745 : }
746 : }
747 :
748 : // governor_reset - disengages governor and resets outputs
749 1220427 : void AP_MotorsHeli_RSC::governor_reset()
750 : {
751 1220427 : _governor_output = 0.0f;
752 1220427 : _governor_torque_reference = 0.0f;
753 1220427 : _governor_engage = false;
754 1220427 : _governor_fault_count = 0; // reset fault count when governor reset
755 1220427 : }
756 :
757 : #if HAL_LOGGING_ENABLED
758 : // Write a helicopter motors packet
759 157035 : void AP_MotorsHeli_RSC::write_log(void) const
760 : {
761 : // @LoggerMessage: HRSC
762 : // @Description: Helicopter related messages
763 : // @Field: TimeUS: Time since system startup
764 : // @Field: I: Instance, 0=Main, 1=Tail
765 : // @Field: DRRPM: Desired rotor speed
766 : // @Field: ERRPM: Estimated rotor speed
767 : // @Field: Gov: Governor Output
768 : // @Field: Throt: Throttle output
769 : // @Field: Ramp: throttle ramp up
770 :
771 : // Write to data flash log
772 314070 : AP::logger().WriteStreaming("HRSC",
773 : "TimeUS,I,DRRPM,ERRPM,Gov,Throt,Ramp",
774 : "s#-----",
775 : "F------",
776 : "QBfffff",
777 : AP_HAL::micros64(),
778 157035 : _instance,
779 157035 : get_desired_rotor_speed(),
780 157035 : _rotor_runup_output,
781 157035 : _governor_output,
782 157035 : get_control_output(),
783 157035 : _rotor_ramp_output);
784 157035 : }
785 : #endif
786 :
787 :
788 : // considered to be "in an autorotation" if active or bailing out
789 15970097 : bool AP_MotorsHeli_RSC::in_autorotation(void) const
790 : {
791 15970097 : return autorotation.active() || autorotation.bailing_out();
792 : }
|
