forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 1
/
AP_ICEngine.cpp
602 lines (520 loc) · 21.4 KB
/
AP_ICEngine.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_ICEngine.h"
#if AP_ICENGINE_ENABLED
#include <SRV_Channel/SRV_Channel.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_Notify/AP_Notify.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_RPM/AP_RPM.h>
#include <AP_Parachute/AP_Parachute.h>
extern const AP_HAL::HAL& hal;
#define AP_ICENGINE_START_CHAN_DEBOUNCE_MS 300
const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable ICEngine control
// @Description: This enables internal combustion engine control
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 0, AP_ICEngine, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: START_CHAN
// @DisplayName: Input channel for engine start
// @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.
// @User: Standard
// @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
AP_GROUPINFO("START_CHAN", 1, AP_ICEngine, start_chan, 0),
// @Param: STARTER_TIME
// @DisplayName: Time to run starter
// @Description: This is the number of seconds to run the starter when trying to start the engine
// @User: Standard
// @Units: s
// @Range: 0.1 5
AP_GROUPINFO("STARTER_TIME", 2, AP_ICEngine, starter_time, 3),
// @Param: START_DELAY
// @DisplayName: Time to wait between starts
// @Description: Delay between start attempts
// @User: Standard
// @Units: s
// @Range: 1 10
AP_GROUPINFO("START_DELAY", 3, AP_ICEngine, starter_delay, 2),
#if AP_RPM_ENABLED
// @Param: RPM_THRESH
// @DisplayName: RPM threshold
// @Description: This is the measured RPM above which the engine is considered to be running
// @User: Standard
// @Range: 100 100000
AP_GROUPINFO("RPM_THRESH", 4, AP_ICEngine, rpm_threshold, 100),
#endif
// @Param: PWM_IGN_ON
// @DisplayName: PWM value for ignition on
// @Description: This is the value sent to the ignition channel when on
// @User: Standard
// @Range: 1000 2000
AP_GROUPINFO("PWM_IGN_ON", 5, AP_ICEngine, pwm_ignition_on, 2000),
// @Param: PWM_IGN_OFF
// @DisplayName: PWM value for ignition off
// @Description: This is the value sent to the ignition channel when off
// @User: Standard
// @Range: 1000 2000
AP_GROUPINFO("PWM_IGN_OFF", 6, AP_ICEngine, pwm_ignition_off, 1000),
// @Param: PWM_STRT_ON
// @DisplayName: PWM value for starter on
// @Description: This is the value sent to the starter channel when on
// @User: Standard
// @Range: 1000 2000
AP_GROUPINFO("PWM_STRT_ON", 7, AP_ICEngine, pwm_starter_on, 2000),
// @Param: PWM_STRT_OFF
// @DisplayName: PWM value for starter off
// @Description: This is the value sent to the starter channel when off
// @User: Standard
// @Range: 1000 2000
AP_GROUPINFO("PWM_STRT_OFF", 8, AP_ICEngine, pwm_starter_off, 1000),
#if AP_RPM_ENABLED
// @Param: RPM_CHAN
// @DisplayName: RPM instance channel to use
// @Description: This is which of the RPM instances to use for detecting the RPM of the engine
// @User: Standard
// @Values: 0:None,1:RPM1,2:RPM2
AP_GROUPINFO("RPM_CHAN", 9, AP_ICEngine, rpm_instance, 0),
#endif
// @Param: START_PCT
// @DisplayName: Throttle percentage for engine start
// @Description: This is the percentage throttle output for engine start
// @User: Standard
// @Range: 0 100
AP_GROUPINFO("START_PCT", 10, AP_ICEngine, start_percent, 5),
// @Param: IDLE_PCT
// @DisplayName: Throttle percentage for engine idle
// @Description: This is the minimum percentage throttle output while running, this includes being disarmed, but not safe
// @User: Standard
// @Range: 0 100
AP_GROUPINFO("IDLE_PCT", 11, AP_ICEngine, idle_percent, 0),
#if AP_RPM_ENABLED
// @Param: IDLE_RPM
// @DisplayName: RPM Setpoint for Idle Governor
// @Description: This configures the RPM that will be commanded by the idle governor. Set to -1 to disable
// @User: Advanced
AP_GROUPINFO("IDLE_RPM", 12, AP_ICEngine, idle_rpm, -1),
// @Param: IDLE_DB
// @DisplayName: Deadband for Idle Governor
// @Description: This configures the deadband that is tolerated before adjusting the idle setpoint
AP_GROUPINFO("IDLE_DB", 13, AP_ICEngine, idle_db, 50),
// @Param: IDLE_SLEW
// @DisplayName: Slew Rate for idle control
// @Description: This configures the slewrate used to adjust the idle setpoint in percentage points per second
AP_GROUPINFO("IDLE_SLEW", 14, AP_ICEngine, idle_slew, 1),
#endif
// @Param: OPTIONS
// @DisplayName: ICE options
// @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.
// @Bitmask: 0:Disable ignition in RC failsafe,1:Disable redline governor,2:Throttle while disarmed,3:Disable while disarmed
AP_GROUPINFO("OPTIONS", 15, AP_ICEngine, options, 0),
// @Param: STARTCHN_MIN
// @DisplayName: Input channel for engine start minimum PWM
// @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 below 1300 triggers an engine stop.
// @User: Standard
// @Range: 0 1300
AP_GROUPINFO("STARTCHN_MIN", 16, AP_ICEngine, start_chan_min_pwm, 0),
#if AP_RPM_ENABLED
// @Param: REDLINE_RPM
// @DisplayName: RPM of the redline limit for the engine
// @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.
// @User: Advanced
// @Range: 0 2000000
// @Units: RPM
AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0),
#endif
AP_GROUPEND
};
// constructor
AP_ICEngine::AP_ICEngine()
{
AP_Param::setup_object_defaults(this, var_info);
if (_singleton != nullptr) {
AP_HAL::panic("AP_ICEngine must be singleton");
}
_singleton = this;
#if AP_RPM_ENABLED
// ICEngine runs at 10Hz
_rpm_filter.set_cutoff_frequency(10, 0.5f);
#endif
}
/*
update engine state
*/
void AP_ICEngine::update(void)
{
if (!enable) {
return;
}
uint16_t cvalue = 1500;
RC_Channel *c = rc().channel(start_chan-1);
if (c != nullptr && rc().has_valid_input()) {
// get starter control channel
cvalue = c->get_radio_in();
if (cvalue < start_chan_min_pwm) {
cvalue = start_chan_last_value;
}
// snap the input to either 1000, 1500, or 2000
// this is useful to compare a debounce changed value
// while ignoring tiny noise
if (cvalue >= RC_Channel::AUX_PWM_TRIGGER_HIGH) {
cvalue = 2000;
} else if ((cvalue > 800) && (cvalue <= RC_Channel::AUX_PWM_TRIGGER_LOW)) {
cvalue = 1300;
} else {
cvalue = 1500;
}
}
bool should_run = false;
uint32_t now = AP_HAL::millis();
// debounce timer to protect from spurious changes on start_chan rc input
// If the cached value is the same, reset timer
if (start_chan_last_value == cvalue) {
start_chan_last_ms = now;
} else if (now - start_chan_last_ms >= AP_ICENGINE_START_CHAN_DEBOUNCE_MS) {
// if it has changed, and stayed changed for the duration, then use that new value
start_chan_last_value = cvalue;
}
if (state == ICE_START_HEIGHT_DELAY && start_chan_last_value >= RC_Channel::AUX_PWM_TRIGGER_HIGH) {
// user is overriding the height start delay and asking for
// immediate start. Put into ICE_OFF so that the logic below
// can start the engine now
state = ICE_OFF;
}
if (state == ICE_OFF && start_chan_last_value >= RC_Channel::AUX_PWM_TRIGGER_HIGH) {
should_run = true;
} else if (start_chan_last_value <= RC_Channel::AUX_PWM_TRIGGER_LOW) {
should_run = false;
// clear the single start flag now that we will be stopping the engine
if (state != ICE_OFF) {
allow_single_start_while_disarmed = false;
}
} else if (state != ICE_OFF) {
should_run = true;
}
if (option_set(Options::DISABLE_IGNITION_RC_FAILSAFE) && AP_Notify::flags.failsafe_radio) {
// user has requested ignition kill on RC failsafe
should_run = false;
}
if (option_set(Options::NO_RUNNING_WHILE_DISARMED)) {
if (hal.util->get_soft_armed()) {
// clear the disarmed start flag, as we are now armed, if we disarm again we expect the engine to stop
allow_single_start_while_disarmed = false;
} else {
// check if we are blocking disarmed starts
if (!allow_single_start_while_disarmed) {
should_run = false;
}
}
}
#if HAL_PARACHUTE_ENABLED
// Stop on parachute deployment
AP_Parachute *parachute = AP::parachute();
if ((parachute != nullptr) && parachute->release_initiated()) {
should_run = false;
}
#endif
// switch on current state to work out new state
switch (state) {
case ICE_DISABLED:
return;
case ICE_OFF:
if (should_run) {
state = ICE_START_DELAY;
}
break;
case ICE_START_HEIGHT_DELAY: {
Vector3f pos;
if (!should_run) {
state = ICE_OFF;
} else if (AP::ahrs().get_relative_position_NED_origin(pos)) {
if (height_pending) {
height_pending = false;
initial_height = -pos.z;
} else if ((-pos.z) >= initial_height + height_required) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting height reached %.1f",
(double)(-pos.z - initial_height));
state = ICE_STARTING;
}
}
break;
}
case ICE_START_DELAY:
if (!should_run) {
state = ICE_OFF;
} else if (now - starter_last_run_ms >= starter_delay*1000) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting engine");
state = ICE_STARTING;
}
break;
case ICE_STARTING:
if (!should_run) {
state = ICE_OFF;
} else if (now - starter_start_time_ms >= starter_time*1000) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine running");
state = ICE_RUNNING;
}
break;
case ICE_RUNNING:
if (!should_run) {
state = ICE_OFF;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Stopped engine");
}
#if AP_RPM_ENABLED
else if (rpm_instance > 0) {
// check RPM to see if still running
float rpm_value;
if (!AP::rpm()->get_rpm(rpm_instance-1, rpm_value) ||
rpm_value < rpm_threshold) {
// engine has stopped when it should be running
state = ICE_START_DELAY;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Uncommanded engine stop");
}
}
#endif
break;
}
if (!hal.util->get_soft_armed()) {
if (state == ICE_START_HEIGHT_DELAY) {
// when disarmed we can be waiting for takeoff
Vector3f pos;
if (AP::ahrs().get_relative_position_NED_origin(pos)) {
// reset initial height while disarmed
initial_height = -pos.z;
}
} else if (idle_percent <= 0 && !option_set(Options::THROTTLE_WHILE_DISARMED)) {
// force ignition off when disarmed
state = ICE_OFF;
}
}
#if AP_RPM_ENABLED
// check against redline RPM
float rpm_value;
if (rpm_instance > 0 && redline_rpm > 0 && AP::rpm()->get_rpm(rpm_instance-1, rpm_value)) {
// update the filtered RPM value
filtered_rpm_value = _rpm_filter.apply(rpm_value);
if (!redline.flag && filtered_rpm_value > redline_rpm) {
// redline governor is off. rpm is too high. enable the governor
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: Above redline RPM");
redline.flag = true;
} else if (redline.flag && filtered_rpm_value < redline_rpm * 0.9f) {
// redline governor is on. rpm is safely below. disable the governor
redline.flag = false;
// reset redline governor
redline.throttle_percentage = 0.0f;
redline.governor_integrator = 0.0f;
}
} else {
redline.flag = false;
}
#endif // AP_RPM_ENABLED
/* now set output channels */
switch (state) {
case ICE_DISABLED:
return;
case ICE_OFF:
SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_off);
SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off);
starter_start_time_ms = 0;
break;
case ICE_START_HEIGHT_DELAY:
case ICE_START_DELAY:
SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on);
SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off);
break;
case ICE_STARTING:
SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on);
SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_on);
if (starter_start_time_ms == 0) {
starter_start_time_ms = now;
}
starter_last_run_ms = now;
break;
case ICE_RUNNING:
SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on);
SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off);
starter_start_time_ms = 0;
break;
}
}
/*
check for throttle override. This allows the ICE controller to force
the correct starting throttle when starting the engine and maintain idle when disarmed
base_throttle is the throttle before the disarmed override
check. This allows for throttle control while disarmed
*/
bool AP_ICEngine::throttle_override(float &percentage, const float base_throttle)
{
if (!enable) {
return false;
}
if (state == ICE_RUNNING &&
idle_percent > 0 &&
idle_percent < 100 &&
idle_percent > percentage)
{
percentage = idle_percent;
if (option_set(Options::THROTTLE_WHILE_DISARMED) && !hal.util->get_soft_armed()) {
percentage = MAX(percentage, base_throttle);
}
return true;
}
if (state == ICE_STARTING || state == ICE_START_DELAY) {
percentage = start_percent.get();
return true;
} else if (state != ICE_RUNNING && hal.util->get_soft_armed()) {
percentage = 0;
return true;
}
#if AP_RPM_ENABLED
if (redline.flag && !option_set(Options::DISABLE_REDLINE_GOVERNOR)) {
// limit the throttle from increasing above what the current output is
if (redline.throttle_percentage < 1.0f) {
redline.throttle_percentage = percentage;
}
if (percentage < redline.throttle_percentage - redline.governor_integrator) {
// the throttle before the override is much lower than what the integrator is at
// reset the integrator
redline.governor_integrator = 0;
redline.throttle_percentage = percentage;
} else if (percentage < redline.throttle_percentage) {
// the throttle is below the integrator set point
// remove the difference from the integrator
redline.governor_integrator -= redline.throttle_percentage - percentage;
redline.throttle_percentage = percentage;
} else if (filtered_rpm_value > redline_rpm) {
// reduce the throttle if still over the redline RPM
const float redline_setpoint_step = idle_slew * AP::scheduler().get_loop_period_s();
redline.governor_integrator += redline_setpoint_step;
}
percentage = redline.throttle_percentage - redline.governor_integrator;
return true;
}
#endif // AP_RPM_ENABLED
// if THROTTLE_WHILE_DISARMED is set then we use the base_throttle, allowing the pilot to control throttle while disarmed
if (option_set(Options::THROTTLE_WHILE_DISARMED) && !hal.util->get_soft_armed() &&
base_throttle > percentage) {
percentage = base_throttle;
return true;
}
return false;
}
/*
handle DO_ENGINE_CONTROL messages via MAVLink or mission
*/
bool AP_ICEngine::engine_control(float start_control, float cold_start, float height_delay, uint32_t flags)
{
if (!enable) {
return false;
}
// always update the start while disarmed flag
allow_single_start_while_disarmed = (flags & ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED) != 0;
if (start_control <= 0) {
state = ICE_OFF;
return true;
}
if (state == ICE_RUNNING || state == ICE_START_DELAY || state == ICE_STARTING) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: already running");
return false;
}
RC_Channel *c = rc().channel(start_chan-1);
if (c != nullptr && rc().has_valid_input()) {
// get starter control channel
uint16_t cvalue = c->get_radio_in();
if (cvalue >= start_chan_min_pwm && cvalue <= RC_Channel::AUX_PWM_TRIGGER_LOW) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: start control disabled");
return false;
}
}
if (height_delay > 0) {
height_pending = true;
initial_height = 0;
height_required = height_delay;
state = ICE_START_HEIGHT_DELAY;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Takeoff height set to %.1fm", (double)height_delay);
return true;
}
state = ICE_STARTING;
return true;
}
/*
Update low throttle limit to ensure steady idle for IC Engines
return a new min_throttle value
*/
void AP_ICEngine::update_idle_governor(int8_t &min_throttle)
{
if (!enable) {
return;
}
#if AP_RPM_ENABLED
const int8_t min_throttle_base = min_throttle;
// Initialize idle point to min_throttle on the first run
static bool idle_point_initialized = false;
if (!idle_point_initialized) {
idle_governor_integrator = min_throttle;
idle_point_initialized = true;
}
AP_RPM *ap_rpm = AP::rpm();
if (!ap_rpm || rpm_instance == 0 || !ap_rpm->healthy(rpm_instance-1)) {
return;
}
// Check to make sure we have an enabled IC Engine, EFI Instance and that the idle governor is enabled
if (get_state() != AP_ICEngine::ICE_RUNNING || idle_rpm < 0) {
return;
}
// get current RPM feedback
float rpmv;
// Double Check to make sure engine is really running
if (!ap_rpm->get_rpm(rpm_instance-1, rpmv) || rpmv < 1) {
// Reset idle point to the default value when the engine is stopped
idle_governor_integrator = min_throttle;
return;
}
// Override
min_throttle = roundf(idle_governor_integrator);
// Calculate Error in system
int32_t error = idle_rpm - rpmv;
bool underspeed = error > 0;
// Don't adjust idle point when we're within the deadband
if (abs(error) < idle_db) {
return;
}
// Don't adjust idle point if the commanded throttle is above the
// current idle throttle setpoint and the RPM is above the idle
// RPM setpoint (Normal flight)
if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > min_throttle && !underspeed) {
return;
}
// Calculate the change per loop to achieve the desired slew rate of 1 percent per second
static const float idle_setpoint_step = idle_slew * AP::scheduler().get_loop_period_s();
// Update Integrator
if (underspeed) {
idle_governor_integrator += idle_setpoint_step;
} else {
idle_governor_integrator -= idle_setpoint_step;
}
idle_governor_integrator = constrain_float(idle_governor_integrator, min_throttle_base, 40.0f);
min_throttle = roundf(idle_governor_integrator);
#endif // AP_RPM_ENABLED
}
// singleton instance. Should only ever be set in the constructor.
AP_ICEngine *AP_ICEngine::_singleton;
namespace AP {
AP_ICEngine *ice() {
return AP_ICEngine::get_singleton();
}
}
#endif // AP_ICENGINE_ENABLED