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 "AP_ICEngine.h"
17 :
18 : #if AP_ICENGINE_ENABLED
19 :
20 : #include <SRV_Channel/SRV_Channel.h>
21 : #include <GCS_MAVLink/GCS.h>
22 : #include <AP_AHRS/AP_AHRS.h>
23 : #include <AP_Scheduler/AP_Scheduler.h>
24 : #include <AP_Notify/AP_Notify.h>
25 : #include <RC_Channel/RC_Channel.h>
26 : #include <AP_RPM/AP_RPM.h>
27 : #include <AP_Parachute/AP_Parachute.h>
28 : #include <AP_Relay/AP_Relay.h>
29 : #include "AP_ICEngine.h"
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
34 :
35 : // @Param: ENABLE
36 : // @DisplayName: Enable ICEngine control
37 : // @Description: This enables internal combustion engine control
38 : // @Values: 0:Disabled, 1:Enabled
39 : // @User: Advanced
40 : AP_GROUPINFO_FLAGS("ENABLE", 0, AP_ICEngine, enable, 0, AP_PARAM_FLAG_ENABLE),
41 :
42 : // @Param: START_CHAN
43 : // @DisplayName: Input channel for engine start
44 : // @Description: This is an RC input channel for requesting engine start. Engine will try to start when channel is at or above 1700. Engine will stop when channel is at or below 1300. Between 1301 and 1699 the engine will not change state unless a MAVLink command or mission item commands a state change, or the vehicle is disarmed. See ICE_STARTCHN_MIN parameter to change engine stop PWM value and/or to enable debouncing of the START_CH to avoid accidental engine kills due to noise on channel.
45 : // @Legacy: 4.5 param
46 : // @User: Standard
47 : // @Values: 0:None,1:Chan1,2:Chan2,3:Chan3,4:Chan4,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
48 :
49 : // 1 was START_CHAN
50 :
51 : // @Param: STARTER_TIME
52 : // @DisplayName: Time to run starter
53 : // @Description: This is the number of seconds to run the starter when trying to start the engine
54 : // @User: Standard
55 : // @Units: s
56 : // @Range: 0.1 5
57 : AP_GROUPINFO("STARTER_TIME", 2, AP_ICEngine, starter_time, 3),
58 :
59 : // @Param: START_DELAY
60 : // @DisplayName: Time to wait between starts
61 : // @Description: Delay between start attempts
62 : // @User: Standard
63 : // @Units: s
64 : // @Range: 1 10
65 : AP_GROUPINFO("START_DELAY", 3, AP_ICEngine, starter_delay, 2),
66 :
67 : #if AP_RPM_ENABLED
68 : // @Param: RPM_THRESH
69 : // @DisplayName: RPM threshold
70 : // @Description: This is the measured RPM above which the engine is considered to be running
71 : // @User: Standard
72 : // @Range: 100 100000
73 : AP_GROUPINFO("RPM_THRESH", 4, AP_ICEngine, rpm_threshold, 100),
74 : #endif
75 :
76 : // @Param: PWM_IGN_ON
77 : // @DisplayName: PWM value for ignition on
78 : // @Description: This is the value sent to the ignition channel when on
79 : // @Legacy: 4.5 param
80 : // @User: Standard
81 : // @Range: 1000 2000
82 :
83 : // 5 was PWM_IGN_ON
84 :
85 : // @Param: PWM_IGN_OFF
86 : // @DisplayName: PWM value for ignition off
87 : // @Description: This is the value sent to the ignition channel when off
88 : // @Legacy: 4.5 param
89 : // @User: Standard
90 : // @Range: 1000 2000
91 :
92 : // 6 was PWM_IGN_OFF
93 :
94 : // @Param: PWM_STRT_ON
95 : // @DisplayName: PWM value for starter on
96 : // @Description: This is the value sent to the starter channel when on
97 : // @Legacy: 4.5 param
98 : // @User: Standard
99 : // @Range: 1000 2000
100 :
101 : // 7 was PWM_STRT_ON
102 :
103 : // @Param: PWM_STRT_OFF
104 : // @DisplayName: PWM value for starter off
105 : // @Description: This is the value sent to the starter channel when off
106 : // @Legacy: 4.5 param
107 : // @User: Standard
108 : // @Range: 1000 2000
109 :
110 : // 8 was PWM_STRT_OFF
111 :
112 : #if AP_RPM_ENABLED
113 : // @Param: RPM_CHAN
114 : // @DisplayName: RPM instance channel to use
115 : // @Description: This is which of the RPM instances to use for detecting the RPM of the engine
116 : // @User: Standard
117 : // @Values: 0:None,1:RPM1,2:RPM2
118 : AP_GROUPINFO("RPM_CHAN", 9, AP_ICEngine, rpm_instance, 0),
119 : #endif
120 :
121 : // @Param: START_PCT
122 : // @DisplayName: Throttle percentage for engine start
123 : // @Description: This is the percentage throttle output for engine start
124 : // @User: Standard
125 : // @Range: 0 100
126 : AP_GROUPINFO("START_PCT", 10, AP_ICEngine, start_percent, 5),
127 :
128 : // @Param: IDLE_PCT
129 : // @DisplayName: Throttle percentage for engine idle
130 : // @Description: This is the minimum percentage throttle output while running, this includes being disarmed, but not safe
131 : // @User: Standard
132 : // @Range: 0 100
133 : AP_GROUPINFO("IDLE_PCT", 11, AP_ICEngine, idle_percent, 0),
134 :
135 : #if AP_RPM_ENABLED
136 : // @Param: IDLE_RPM
137 : // @DisplayName: RPM Setpoint for Idle Governor
138 : // @Description: This configures the RPM that will be commanded by the idle governor. Set to -1 to disable
139 : // @User: Advanced
140 : AP_GROUPINFO("IDLE_RPM", 12, AP_ICEngine, idle_rpm, -1),
141 :
142 : // @Param: IDLE_DB
143 : // @DisplayName: Deadband for Idle Governor
144 : // @Description: This configures the deadband that is tolerated before adjusting the idle setpoint
145 : AP_GROUPINFO("IDLE_DB", 13, AP_ICEngine, idle_db, 50),
146 :
147 : // @Param: IDLE_SLEW
148 : // @DisplayName: Slew Rate for idle control
149 : // @Description: This configures the slewrate used to adjust the idle setpoint in percentage points per second
150 : AP_GROUPINFO("IDLE_SLEW", 14, AP_ICEngine, idle_slew, 1),
151 : #endif
152 :
153 : // @Param: OPTIONS
154 : // @DisplayName: ICE options
155 : // @Description: Options for ICE control. The Disable ignition in RC failsafe option will cause the ignition to be set off on any R/C failsafe. If Throttle while disarmed is set then throttle control will be allowed while disarmed for planes when in MANUAL mode. If disable while disarmed is set the engine will not start while the vehicle is disarmed unless overriden by the MAVLink DO_ENGINE_CONTROL command.
156 : // @Bitmask: 0:Disable ignition in RC failsafe,1:Disable redline governor,2:Throttle control in MANUAL while disarmed with safety off,3:Disable while disarmed,4:Crank direction Reverse
157 : AP_GROUPINFO("OPTIONS", 15, AP_ICEngine, options, 0),
158 :
159 : // @Param: STARTCHN_MIN
160 : // @DisplayName: Input channel for engine start minimum PWM
161 : // @Description: This is a minimum PWM value for engine start channel for an engine stop to be commanded. Setting this value will avoid RC input glitches with low PWM values from causing an unwanted engine stop. A value of zero means any PWM above 800 and below 1300 triggers an engine stop. To stop the engine start channel must above the larger of this value and 800 and below 1300.
162 : // @User: Standard
163 : // @Range: 0 1300
164 : AP_GROUPINFO("STARTCHN_MIN", 16, AP_ICEngine, start_chan_min_pwm, 0),
165 :
166 : #if AP_RPM_ENABLED
167 : // @Param: REDLINE_RPM
168 : // @DisplayName: RPM of the redline limit for the engine
169 : // @Description: Maximum RPM for the engine provided by the manufacturer. A value of 0 disables this feature. See ICE_OPTIONS to enable or disable the governor.
170 : // @User: Advanced
171 : // @Range: 0 2000000
172 : // @Units: RPM
173 : AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0),
174 : #endif
175 :
176 : // 18 was IGNITION_RLY
177 :
178 : // Hidden param used as a flag for param conversion
179 : // This allows one time conversion while allowing user to flash between versions with and without converted params
180 : AP_GROUPINFO_FLAGS("FMT_VER", 19, AP_ICEngine, param_format_version, 0, AP_PARAM_FLAG_HIDDEN),
181 :
182 : // @Param: STRT_MX_RTRY
183 : // @DisplayName: Maximum number of retries
184 : // @Description: If set 0 then there is no limit to retrials. If set to a value greater than 0 then the engine will retry starting the engine this many times before giving up.
185 : // @User: Standard
186 : // @Range: 0 127
187 : AP_GROUPINFO("STRT_MX_RTRY", 20, AP_ICEngine, max_crank_retry, 0),
188 :
189 : AP_GROUPEND
190 : };
191 :
192 : // constructor
193 692 : AP_ICEngine::AP_ICEngine()
194 : {
195 692 : AP_Param::setup_object_defaults(this, var_info);
196 :
197 692 : if (_singleton != nullptr) {
198 0 : AP_HAL::panic("AP_ICEngine must be singleton");
199 : }
200 692 : _singleton = this;
201 :
202 : #if AP_RPM_ENABLED
203 : // ICEngine runs at 10Hz
204 692 : _rpm_filter.set_cutoff_frequency(10, 0.5f);
205 : #endif
206 692 : }
207 :
208 : // One time init call
209 691 : void AP_ICEngine::init()
210 : {
211 : // Configure starter and ignition outputs as range type
212 691 : SRV_Channels::set_range(SRV_Channel::k_starter, 1);
213 691 : SRV_Channels::set_range(SRV_Channel::k_ignition, 1);
214 :
215 : // Set default PWM endpoints to 1000 to 2000
216 691 : SRV_Channels::set_output_min_max_defaults(SRV_Channel::k_starter, 1000, 2000);
217 691 : SRV_Channels::set_output_min_max_defaults(SRV_Channel::k_ignition, 1000, 2000);
218 :
219 : // Convert params
220 691 : param_conversion();
221 691 : }
222 :
223 : // PARAMETER_CONVERSION - Added: Aug-2024 for ArduPilot-4.6
224 691 : void AP_ICEngine::param_conversion()
225 : {
226 691 : if (!enable || (param_format_version == 1)) {
227 : // not enabled or conversion has already been done
228 685 : return;
229 : }
230 :
231 : // Set format version so the conversion is not done again
232 6 : param_format_version.set_and_save(1);
233 :
234 : AP_Param::ConversionInfo info;
235 6 : if (!AP_Param::find_key_by_pointer(this, info.old_key)) {
236 0 : return;
237 : }
238 :
239 : // Conversion table giving the old on and off pwm parameter indexes and the function for both starter and ignition
240 : const struct convert_table {
241 : uint32_t element[2];
242 : SRV_Channel::Function fuction;
243 6 : } conversion_table[] = {
244 : { {450, 514}, SRV_Channel::k_starter }, // PWM_STRT_ON, PWM_STRT_OFF
245 : { {322, 386}, SRV_Channel::k_ignition }, // PWM_IGN_ON, PWM_IGN_OFF
246 : };
247 :
248 : // All PWM values were int16
249 6 : info.type = AP_PARAM_INT16;
250 :
251 18 : for (const auto & elem : conversion_table) {
252 : // Use the original default values if params are not saved
253 12 : uint16_t pwm_on = 2000;
254 12 : uint16_t pwm_off = 1000;
255 :
256 : // Get param values if configured
257 12 : AP_Int16 param_value;
258 12 : info.old_group_element = elem.element[0];
259 12 : if (AP_Param::find_old_parameter(&info, ¶m_value)) {
260 0 : pwm_on = param_value;
261 : }
262 12 : info.old_group_element = elem.element[1];
263 12 : if (AP_Param::find_old_parameter(&info, ¶m_value)) {
264 0 : pwm_off = param_value;
265 : }
266 :
267 : // Save as servo endpoints, note that save_output_min_max will set reversed if needed
268 12 : SRV_Channels::save_output_min_max(elem.fuction, pwm_off, pwm_on);
269 : }
270 :
271 : // Convert to new RC option
272 6 : AP_Int8 start_chan;
273 6 : info.type = AP_PARAM_INT8;
274 6 : info.old_group_element = 66;
275 6 : if (AP_Param::find_old_parameter(&info, &start_chan) && (start_chan > 0)) {
276 0 : RC_Channel *chan = rc().channel(start_chan-1);
277 0 : if (chan != nullptr) {
278 0 : chan->option.set_and_save((int16_t)RC_Channel::AUX_FUNC::ICE_START_STOP);
279 : }
280 : }
281 : }
282 :
283 : // Handle incoming aux function
284 28 : void AP_ICEngine::do_aux_function(const RC_Channel::AuxFuncTrigger &trigger)
285 : {
286 : // If triggered from RC apply start chan min
287 28 : if (trigger.source == RC_Channel::AuxFuncTrigger::Source::RC) {
288 20 : RC_Channel *chan = rc().channel(trigger.source_index);
289 20 : if ((chan != nullptr) && (chan->get_radio_in() < start_chan_min_pwm)) {
290 0 : return;
291 : }
292 : }
293 :
294 28 : aux_pos = trigger.pos;
295 : }
296 :
297 : /*
298 : update engine state
299 : */
300 547089 : void AP_ICEngine::update(void)
301 : {
302 547089 : if (!enable) {
303 535579 : return;
304 : }
305 :
306 11510 : bool should_run = false;
307 11510 : uint32_t now = AP_HAL::millis();
308 :
309 :
310 11510 : if ((state == ICE_START_HEIGHT_DELAY) && (aux_pos == RC_Channel::AuxSwitchPos::HIGH)) {
311 : // user is overriding the height start delay and asking for
312 : // immediate start. Put into ICE_OFF so that the logic below
313 : // can start the engine now
314 0 : state = ICE_OFF;
315 : }
316 :
317 11510 : if ((state == ICE_OFF) && (aux_pos == RC_Channel::AuxSwitchPos::HIGH)) {
318 34 : should_run = true;
319 11476 : } else if (aux_pos == RC_Channel::AuxSwitchPos::LOW) {
320 134 : should_run = false;
321 :
322 : // clear the single start flag now that we will be stopping the engine
323 134 : if (state != ICE_OFF) {
324 3 : allow_single_start_while_disarmed = false;
325 : }
326 11342 : } else if (state != ICE_OFF) {
327 9268 : should_run = true;
328 : }
329 :
330 11510 : if (option_set(Options::DISABLE_IGNITION_RC_FAILSAFE) && AP_Notify::flags.failsafe_radio) {
331 : // user has requested ignition kill on RC failsafe
332 0 : should_run = false;
333 : }
334 :
335 11510 : if (option_set(Options::NO_RUNNING_WHILE_DISARMED)) {
336 0 : if (hal.util->get_soft_armed()) {
337 : // clear the disarmed start flag, as we are now armed, if we disarm again we expect the engine to stop
338 0 : allow_single_start_while_disarmed = false;
339 : } else {
340 : // check if we are blocking disarmed starts
341 0 : if (!allow_single_start_while_disarmed) {
342 0 : should_run = false;
343 : }
344 : }
345 : }
346 :
347 : #if HAL_PARACHUTE_ENABLED
348 : // Stop on parachute deployment
349 11510 : AP_Parachute *parachute = AP::parachute();
350 11510 : if ((parachute != nullptr) && parachute->release_initiated()) {
351 0 : should_run = false;
352 : }
353 : #endif
354 :
355 : // Stop on emergency stop
356 11510 : if (SRV_Channels::get_emergency_stop()) {
357 : // Throttle is already forced to 0 in this case, ignition should also be stopped.
358 : // Starter should not run.
359 0 : should_run = false;
360 : }
361 :
362 : // switch on current state to work out new state
363 11510 : switch (state) {
364 0 : case ICE_DISABLED:
365 0 : return;
366 2239 : case ICE_OFF:
367 2239 : if (should_run) {
368 34 : state = ICE_START_DELAY;
369 : }
370 2239 : crank_retry_ct = 0;
371 : // clear the last uncommanded stop time, we only care about tracking
372 : // the last one since the engine was started
373 2239 : last_uncommanded_stop_ms = 0;
374 2239 : break;
375 :
376 239 : case ICE_START_HEIGHT_DELAY: {
377 239 : Vector3f pos;
378 239 : if (!should_run) {
379 0 : state = ICE_OFF;
380 239 : } else if (AP::ahrs().get_relative_position_NED_origin_float(pos)) {
381 239 : if (height_pending) {
382 2 : height_pending = false;
383 2 : initial_height = -pos.z;
384 237 : } else if ((-pos.z) >= initial_height + height_required) {
385 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting height reached %.1f",
386 : (double)(-pos.z - initial_height));
387 2 : state = ICE_STARTING;
388 : }
389 : }
390 239 : break;
391 : }
392 :
393 799 : case ICE_START_DELAY:
394 799 : if (!should_run) {
395 2 : state = ICE_OFF;
396 797 : } else if (now - starter_last_run_ms >= starter_delay*1000) {
397 : // check if we should retry starting the engine
398 49 : if (max_crank_retry <= 0 || crank_retry_ct < max_crank_retry) {
399 18 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting engine");
400 18 : state = ICE_STARTING;
401 18 : crank_retry_ct++;
402 : } else {
403 31 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Engine max crank attempts reached");
404 : // Mark the last run now so we don't send this message every loop
405 31 : starter_last_run_ms = now;
406 : }
407 : }
408 799 : break;
409 :
410 613 : case ICE_STARTING:
411 613 : if (!should_run) {
412 0 : state = ICE_OFF;
413 613 : } else if (now - starter_start_time_ms >= starter_time*1000) {
414 25 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine running");
415 25 : state = ICE_RUNNING;
416 : }
417 613 : break;
418 :
419 7620 : case ICE_RUNNING:
420 7620 : if (!should_run) {
421 1 : state = ICE_OFF;
422 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Stopped engine");
423 : }
424 : #if AP_RPM_ENABLED
425 7619 : else if (rpm_instance > 0) {
426 : // check RPM to see if still running
427 : float rpm_value;
428 15228 : if (!AP::rpm()->get_rpm(rpm_instance-1, rpm_value) ||
429 7609 : rpm_value < rpm_threshold) {
430 : // engine has stopped when it should be running
431 15 : state = ICE_START_DELAY;
432 15 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Uncommanded engine stop");
433 23 : if (last_uncommanded_stop_ms != 0 &&
434 8 : now - last_uncommanded_stop_ms > 3*(starter_time + starter_delay)*1000) {
435 : // if it has been a long enough time since the last uncommanded stop
436 : // (3 times the time between start attempts) then reset the retry count
437 1 : crank_retry_ct = 0;
438 : }
439 15 : last_uncommanded_stop_ms = now;
440 : }
441 : }
442 : #endif
443 7620 : break;
444 : }
445 :
446 11510 : if (!hal.util->get_soft_armed()) {
447 3129 : if (state == ICE_START_HEIGHT_DELAY) {
448 : // when disarmed we can be waiting for takeoff
449 0 : Vector3f pos;
450 0 : if (AP::ahrs().get_relative_position_NED_origin_float(pos)) {
451 : // reset initial height while disarmed
452 0 : initial_height = -pos.z;
453 : }
454 3129 : } else if (idle_percent <= 0 && !allow_throttle_while_disarmed()) {
455 : // force ignition off when disarmed
456 1187 : state = ICE_OFF;
457 : }
458 : }
459 :
460 : #if AP_RPM_ENABLED
461 : // check against redline RPM
462 : float rpm_value;
463 11510 : if (rpm_instance > 0 && redline_rpm > 0 && AP::rpm()->get_rpm(rpm_instance-1, rpm_value)) {
464 : // update the filtered RPM value
465 478 : filtered_rpm_value = _rpm_filter.apply(rpm_value);
466 478 : if (!redline.flag && filtered_rpm_value > redline_rpm) {
467 : // redline governor is off. rpm is too high. enable the governor
468 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: Above redline RPM");
469 1 : redline.flag = true;
470 477 : } else if (redline.flag && filtered_rpm_value < redline_rpm * 0.9f) {
471 : // redline governor is on. rpm is safely below. disable the governor
472 1 : redline.flag = false;
473 : // reset redline governor
474 1 : redline.throttle_percentage = 0.0f;
475 1 : redline.governor_integrator = 0.0f;
476 : }
477 : } else {
478 11032 : redline.flag = false;
479 : }
480 : #endif // AP_RPM_ENABLED
481 :
482 : /* now set output channels */
483 11510 : switch (state) {
484 0 : case ICE_DISABLED:
485 0 : return;
486 2240 : case ICE_OFF:
487 2240 : set_ignition(false);
488 2240 : set_starter(false);
489 2240 : starter_start_time_ms = 0;
490 2240 : break;
491 :
492 1036 : case ICE_START_HEIGHT_DELAY:
493 : case ICE_START_DELAY:
494 1036 : set_ignition(true);
495 1036 : set_starter(false);
496 1036 : break;
497 :
498 608 : case ICE_STARTING:
499 608 : set_ignition(true);
500 608 : set_starter(true);
501 :
502 608 : if (starter_start_time_ms == 0) {
503 20 : starter_start_time_ms = now;
504 : }
505 608 : starter_last_run_ms = now;
506 608 : break;
507 :
508 7626 : case ICE_RUNNING:
509 7626 : set_ignition(true);
510 7626 : set_starter(false);
511 7626 : starter_start_time_ms = 0;
512 7626 : break;
513 : }
514 : }
515 :
516 :
517 : /*
518 : check for throttle override. This allows the ICE controller to force
519 : the correct starting throttle when starting the engine and maintain idle when disarmed
520 :
521 : base_throttle is the throttle before the disarmed override
522 : check. This allows for throttle control while disarmed
523 : */
524 8377244 : bool AP_ICEngine::throttle_override(float &percentage, const float base_throttle)
525 : {
526 8377244 : if (!enable) {
527 8078727 : return false;
528 : }
529 :
530 298517 : min_throttle_pct = idle_percent.get();
531 : #if AP_RPM_ENABLED
532 298517 : update_idle_governor(min_throttle_pct);
533 : #endif // AP_RPM_ENABLED
534 :
535 298517 : if (state == ICE_RUNNING &&
536 188397 : min_throttle_pct > 0 &&
537 28690 : min_throttle_pct < 100 &&
538 28690 : min_throttle_pct > percentage)
539 : {
540 14830 : percentage = min_throttle_pct;
541 14830 : if (allow_throttle_while_disarmed() && !hal.util->get_soft_armed()) {
542 14829 : percentage = MAX(percentage, base_throttle);
543 : }
544 14830 : return true;
545 : }
546 :
547 283687 : if (state == ICE_STARTING || state == ICE_START_DELAY) {
548 41504 : percentage = start_percent.get();
549 41504 : return true;
550 242183 : } else if (state != ICE_RUNNING && hal.util->get_soft_armed()) {
551 37330 : percentage = 0;
552 37330 : return true;
553 : }
554 :
555 : #if AP_RPM_ENABLED
556 204853 : if (redline.flag && !option_set(Options::DISABLE_REDLINE_GOVERNOR)) {
557 : // limit the throttle from increasing above what the current output is
558 13390 : if (redline.throttle_percentage < 1.0f) {
559 1 : redline.throttle_percentage = percentage;
560 : }
561 13390 : if (percentage < redline.throttle_percentage - redline.governor_integrator) {
562 : // the throttle before the override is much lower than what the integrator is at
563 : // reset the integrator
564 2 : redline.governor_integrator = 0;
565 2 : redline.throttle_percentage = percentage;
566 13388 : } else if (percentage < redline.throttle_percentage) {
567 : // the throttle is below the integrator set point
568 : // remove the difference from the integrator
569 0 : redline.governor_integrator -= redline.throttle_percentage - percentage;
570 0 : redline.throttle_percentage = percentage;
571 13388 : } else if (filtered_rpm_value > redline_rpm) {
572 : // reduce the throttle if still over the redline RPM
573 4432 : const float redline_setpoint_step = idle_slew * AP::scheduler().get_loop_period_s();
574 4432 : redline.governor_integrator += redline_setpoint_step;
575 : }
576 13390 : percentage = redline.throttle_percentage - redline.governor_integrator;
577 13390 : return true;
578 : }
579 : #endif // AP_RPM_ENABLED
580 :
581 : // if THROTTLE_WHILE_DISARMED is set then we use the base_throttle, allowing the pilot to control throttle while disarmed
582 201086 : if (allow_throttle_while_disarmed() && !hal.util->get_soft_armed() &&
583 9623 : base_throttle > percentage) {
584 116 : percentage = base_throttle;
585 116 : return true;
586 : }
587 :
588 191347 : return false;
589 : }
590 :
591 : /*
592 : handle DO_ENGINE_CONTROL messages via MAVLink or mission
593 : */
594 103 : bool AP_ICEngine::engine_control(float start_control, float cold_start, float height_delay, uint32_t flags)
595 : {
596 103 : if (!enable) {
597 82 : return false;
598 : }
599 :
600 : // always update the start while disarmed flag
601 21 : allow_single_start_while_disarmed = (flags & ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED) != 0;
602 :
603 21 : if (start_control <= 0) {
604 10 : state = ICE_OFF;
605 10 : return true;
606 : }
607 11 : if (state == ICE_RUNNING || state == ICE_START_DELAY || state == ICE_STARTING) {
608 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: already running");
609 2 : return false;
610 : }
611 :
612 : // get starter control channel
613 9 : if (aux_pos == RC_Channel::AuxSwitchPos::LOW) {
614 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: start control disabled by aux function");
615 2 : return false;
616 : }
617 :
618 7 : if (height_delay > 0) {
619 2 : height_pending = true;
620 2 : initial_height = 0;
621 2 : height_required = height_delay;
622 2 : state = ICE_START_HEIGHT_DELAY;
623 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Takeoff height set to %.1fm", (double)height_delay);
624 2 : return true;
625 : }
626 5 : state = ICE_STARTING;
627 5 : return true;
628 : }
629 :
630 : #if AP_RPM_ENABLED
631 : /*
632 : Update low throttle limit to ensure steady idle for IC Engines
633 : return a new min_throttle value
634 : */
635 298517 : void AP_ICEngine::update_idle_governor(int8_t &min_throttle)
636 : {
637 298517 : if (!enable) {
638 293477 : return;
639 : }
640 298517 : const int8_t min_throttle_base = min_throttle;
641 :
642 : // Initialize idle point to start_percent on the first run
643 : static bool idle_point_initialized = false;
644 298517 : if (!idle_point_initialized) {
645 237116 : idle_governor_integrator = start_percent.get();
646 237116 : idle_point_initialized = true;
647 : }
648 298517 : AP_RPM *ap_rpm = AP::rpm();
649 298517 : if (!ap_rpm || rpm_instance == 0 || !ap_rpm->healthy(rpm_instance-1)) {
650 32711 : return;
651 : }
652 :
653 : // Check to make sure we have an enabled IC Engine, EFI Instance and that the idle governor is enabled
654 265806 : if (get_state() != AP_ICEngine::ICE_RUNNING || idle_rpm < 0) {
655 237116 : idle_point_initialized = false;
656 237116 : return;
657 : }
658 :
659 : // get current RPM feedback
660 : float rpmv;
661 :
662 : // Double Check to make sure engine is really running
663 28690 : if (!ap_rpm->get_rpm(rpm_instance-1, rpmv) || rpmv < 1) {
664 : // Reset idle point to the default value when the engine is stopped
665 0 : idle_point_initialized = false;
666 0 : return;
667 : }
668 :
669 : // Override
670 28690 : min_throttle = roundf(idle_governor_integrator);
671 :
672 : // Calculate Error in system
673 28690 : int32_t error = idle_rpm - rpmv;
674 :
675 28690 : bool underspeed = error > 0;
676 :
677 : // Don't adjust idle point when we're within the deadband
678 28690 : if (abs(error) < idle_db) {
679 9970 : return;
680 : }
681 :
682 : // Don't adjust idle point if the commanded throttle is above the
683 : // current idle throttle setpoint and the RPM is above the idle
684 : // RPM setpoint (Normal flight)
685 18720 : if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > min_throttle && !underspeed) {
686 13680 : return;
687 : }
688 :
689 : // Calculate the change per loop to achieve the desired slew rate of 1 percent per second
690 5040 : static const float idle_setpoint_step = idle_slew * AP::scheduler().get_loop_period_s();
691 :
692 : // Update Integrator
693 5040 : if (underspeed) {
694 4848 : idle_governor_integrator += idle_setpoint_step;
695 : } else {
696 192 : idle_governor_integrator -= idle_setpoint_step;
697 : }
698 :
699 5040 : idle_governor_integrator = constrain_float(idle_governor_integrator, min_throttle_base, 40.0f);
700 :
701 5040 : min_throttle = roundf(idle_governor_integrator);
702 : }
703 : #endif // AP_RPM_ENABLED
704 :
705 : /*
706 : set ignition state
707 : */
708 11510 : void AP_ICEngine::set_ignition(bool on)
709 : {
710 11510 : SRV_Channels::set_output_scaled(SRV_Channel::k_ignition, on ? 1.0 : 0.0);
711 :
712 : #if AP_RELAY_ENABLED
713 11510 : AP_Relay *relay = AP::relay();
714 11510 : if (relay != nullptr) {
715 11510 : relay->set(AP_Relay_Params::FUNCTION::IGNITION, on);
716 : }
717 : #endif // AP_RELAY_ENABLED
718 :
719 11510 : }
720 :
721 : /*
722 : set starter state
723 : */
724 11510 : void AP_ICEngine::set_starter(bool on)
725 : {
726 11510 : SRV_Channels::set_output_scaled(SRV_Channel::k_starter, on ? 1.0 : 0.0);
727 :
728 : #if AP_RELAY_ENABLED
729 11510 : AP_Relay *relay = AP::relay();
730 11510 : if (relay != nullptr) {
731 11510 : relay->set(AP_Relay_Params::FUNCTION::ICE_STARTER, on);
732 : }
733 : #endif // AP_RELAY_ENABLED
734 11510 : }
735 :
736 :
737 8586710 : bool AP_ICEngine::allow_throttle_while_disarmed() const
738 : {
739 8685688 : return option_set(Options::THROTTLE_WHILE_DISARMED) &&
740 8685688 : hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
741 : }
742 :
743 : #if AP_RELAY_ENABLED
744 692 : bool AP_ICEngine::get_legacy_ignition_relay_index(int8_t &num)
745 : {
746 : // PARAMETER_CONVERSION - Added: Dec-2023
747 692 : if (!enable || !AP_Param::get_param_by_index(this, 18, AP_PARAM_INT8, &num)) {
748 692 : return false;
749 : }
750 : // convert to zero indexed
751 0 : num -= 1;
752 0 : return true;
753 : }
754 : #endif
755 :
756 : // singleton instance. Should only ever be set in the constructor.
757 : AP_ICEngine *AP_ICEngine::_singleton;
758 : namespace AP {
759 2103 : AP_ICEngine *ice() {
760 2103 : return AP_ICEngine::get_singleton();
761 : }
762 : }
763 :
764 : #endif // AP_ICENGINE_ENABLED
|
