diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 4f753713f8..37804adf8d 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -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 }; @@ -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: { @@ -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; diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index 93277999f3..9e114a3c65 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -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;