Skip to content

Commit

Permalink
AP_ICEngine: add max retrial of cranking
Browse files Browse the repository at this point in the history
Added Param MAX_RETRY which If set 0 or less, 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.

SW-314
  • Loading branch information
loki077 committed Sep 6, 2024
1 parent 399d84d commit 3f03b98
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 2 deletions.
18 changes: 16 additions & 2 deletions libraries/AP_ICEngine/AP_ICEngine.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -175,6 +175,13 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
// @Values: 0:Forward,1:Reverse
AP_GROUPINFO("CRANK_DIR", 19, AP_ICEngine, crank_direction, 0),

// @Param: STRT_MX_RTRY
// @DisplayName: Maximum number of retries
// @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.
// @User: Standard
// @Range: 0 127
AP_GROUPINFO("STRT_MX_RTRY", 20, AP_ICEngine, max_crank_retry, 0),

AP_GROUPEND
};

Expand Down Expand Up @@ -291,6 +298,7 @@ void AP_ICEngine::update(void)
if (should_run) {
state = ICE_START_DELAY;
}
crank_retry_ct = 0;
break;

case ICE_START_HEIGHT_DELAY: {
Expand All @@ -314,8 +322,14 @@ void AP_ICEngine::update(void)
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;
// check if we should retry starting the engine
if (max_crank_retry <= 0 || crank_retry_ct < max_crank_retry) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting engine");
state = ICE_STARTING;
crank_retry_ct++;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine Max Crank Retry reached");
}
}
break;

Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_ICEngine/AP_ICEngine.h
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,11 @@ class AP_ICEngine {
AP_Int16 pwm_starter_on;
AP_Int16 pwm_starter_off;


// max crank retry
AP_Int8 max_crank_retry;
int8_t crank_retry_ct;

#if AP_RPM_ENABLED
// RPM above which engine is considered to be running
AP_Int32 rpm_threshold;
Expand Down

0 comments on commit 3f03b98

Please sign in to comment.