From df76c980bdb906954a087273b54cfb63f218f39e Mon Sep 17 00:00:00 2001 From: Loki077 Date: Tue, 3 Sep 2024 15:59:27 +1000 Subject: [PATCH 1/3] AP_ICEngine: add max retrial of cranking 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. --- libraries/AP_ICEngine/AP_ICEngine.cpp | 30 +++++++++++++++++++++++++-- libraries/AP_ICEngine/AP_ICEngine.h | 7 +++++++ 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index b96f717875a76..def34e5460c0c 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -181,6 +181,13 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // This allows one time conversion while allowing user to flash between versions with and without converted params AP_GROUPINFO_FLAGS("FMT_VER", 19, AP_ICEngine, param_format_version, 0, AP_PARAM_FLAG_HIDDEN), + // @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 }; @@ -379,6 +386,10 @@ void AP_ICEngine::update(void) if (should_run) { state = ICE_START_DELAY; } + crank_retry_ct = 0; + // clear the last uncommanded stop time, we only care about tracking + // the last one since the engine was started + last_uncommanded_stop_ms = 0; break; case ICE_START_HEIGHT_DELAY: { @@ -402,8 +413,16 @@ 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 attempts reached"); + // Mark the last run now so we don't send this message every loop + starter_last_run_ms = now; + } } break; @@ -430,6 +449,13 @@ void AP_ICEngine::update(void) // engine has stopped when it should be running state = ICE_START_DELAY; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Uncommanded engine stop"); + if (last_uncommanded_stop_ms != 0 && + now - last_uncommanded_stop_ms > 3*(starter_time + starter_delay)*1000) { + // if it has been a long enough time since the last uncommanded stop + // (3 times the time between start attempts) then reset the retry count + crank_retry_ct = 0; + } + last_uncommanded_stop_ms = now; } } #endif diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index 159ee99bb9058..4afc96f2ada6d 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -107,6 +107,10 @@ class AP_ICEngine { // delay between start attempts (seconds) AP_Float starter_delay; + // 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; @@ -118,6 +122,9 @@ class AP_ICEngine { // time when we last ran the starter uint32_t starter_last_run_ms; + // time when we last had an uncommanded engine stop + uint32_t last_uncommanded_stop_ms; + // throttle percentage for engine start AP_Int8 start_percent; From eee60531c7631c8a6acb3d268ebbc6ae056f35d1 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 17 Sep 2024 10:39:07 +1000 Subject: [PATCH 2/3] Tools: autotest: test ICE max starter retry limit --- Tools/autotest/quadplane.py | 47 +++++++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index f73a7918906bb..a5a61fe763f79 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1116,6 +1116,53 @@ def ICEngine(self): self.change_mode('FBWA') self.wait_servo_channel_value(3, 2000) + self.start_subtest("Testing automatic restart") + # Limit start attempts to 4 + max_tries = 4 + self.set_parameter("ICE_STRT_MX_RTRY", max_tries) + # Make the engine unable to run (by messing up the RPM sensor) + rpm_chan = self.get_parameter("ICE_RPM_CHAN") + self.set_parameter("ICE_RPM_CHAN", 120) # Set to a non-existent sensor + self.set_rc(rc_engine_start_chan, 2000) + self.wait_statustext("Uncommanded engine stop") + self.wait_statustext("Starting engine") + # Restore the engine + self.set_parameter("ICE_RPM_CHAN", rpm_chan) + # Make sure the engine continues to run for the next 30 seconds + try: + self.wait_statustext("Uncommanded engine stop", timeout=30) + # The desired result is for the wait_statustext raise AutoTestTimeoutException + raise NotAchievedException("Engine stopped unexpectedly") + except AutoTestTimeoutException: + pass + self.context_stop_collecting("STATUSTEXT") + + self.start_subtest("Testing automatic starter attempt limit") + # Try this test twice. + # For the first run, since the engine has been running successfully in + # the previous test for 30 seconds, the limit should reset. For the + # second run, after commanding an engine stop, the limit should reset. + for i in range(2): + self.context_collect("STATUSTEXT") + self.set_parameter("ICE_RPM_CHAN", 120) # Set to a non-existent sensor + self.set_rc(rc_engine_start_chan, 2000) + self.wait_statustext("Engine max crank attempts reached", check_context=True, timeout=30) + self.delay_sim_time(30) # wait for another 30 seconds to make sure the engine doesn't restart + messages = self.context_get().collections["STATUSTEXT"] + self.context_stop_collecting("STATUSTEXT") + # check for the exact number of starter attempts + attempts = 0 + for m in messages: + if "Starting engine" == m.text: + attempts += 1 + if attempts != max_tries: + raise NotAchievedException(f"Run {i+1}: Expected {max_tries} attempts, got {attempts}") + # Command an engine stop + self.context_collect("STATUSTEXT") + self.set_rc(rc_engine_start_chan, 1000) + self.wait_statustext("ignition:0", check_context=True) + self.context_stop_collecting("STATUSTEXT") + def ICEngineMission(self): '''Test ICE Engine Mission support''' rc_engine_start_chan = 11 From bcc04c445eeb71433ef2bc6369fc01185b9616d2 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 26 Sep 2024 15:05:54 +1000 Subject: [PATCH 3/3] AP_ICEngine: fix send_text severities --- libraries/AP_ICEngine/AP_ICEngine.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index def34e5460c0c..9e3dde0a6dc4e 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -419,7 +419,7 @@ void AP_ICEngine::update(void) state = ICE_STARTING; crank_retry_ct++; } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine max crank attempts reached"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Engine max crank attempts reached"); // Mark the last run now so we don't send this message every loop starter_last_run_ms = now; } @@ -448,7 +448,7 @@ void AP_ICEngine::update(void) 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"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Uncommanded engine stop"); if (last_uncommanded_stop_ms != 0 && now - last_uncommanded_stop_ms > 3*(starter_time + starter_delay)*1000) { // if it has been a long enough time since the last uncommanded stop