From f20ec45f7f797ded0769c75eb42e65950b33f022 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Wed, 30 Sep 2026 09:17:26 -0700 Subject: [PATCH] fix(commander): decouple ESC arming timeout from ESC telemetry timeout (#28915) * fix(commander): decouple ESC arming timeout from ESC telemetry timeout * fix(escCheck): increase arming timeout to be more permissive for certain ESCs * fix(escChecks): small additions to the unit tests --------- Co-authored-by: Matthias Grob --- .../HealthAndArmingChecks/CMakeLists.txt | 1 + .../HealthAndArmingChecks/checks/escCheck.cpp | 4 +- .../HealthAndArmingChecks/checks/escCheck.hpp | 10 +- .../HealthAndArmingChecks/escChecksTest.cpp | 169 ++++++++++++++++++ 4 files changed, 179 insertions(+), 5 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index f3966517799..101b27346ce 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -91,5 +91,6 @@ px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp px4_add_functional_gtest(SRC gnssRedundancyChecksTest.cpp LINKLIBS health_and_arming_checks mode_util) px4_add_functional_gtest(SRC estimatorChecksTest.cpp LINKLIBS health_and_arming_checks mode_util) +px4_add_functional_gtest(SRC escChecksTest.cpp LINKLIBS health_and_arming_checks mode_util) px4_add_functional_gtest(SRC trafficAvoidanceCheckTest.cpp LINKLIBS health_and_arming_checks mode_util) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index ac459c78f84..57580c363ab 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -127,7 +127,7 @@ uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, con continue; // Skip unmapped ESC status entries } - const bool esc_telemetry_timeout = now > esc_status.esc[esc_index].timestamp + ESC_TIMEOUT_US; + const bool esc_telemetry_timeout = now > esc_status.esc[esc_index].timestamp + ESC_OFFLINE_TIMEOUT_US; const bool is_offline = (esc_status.esc_online_flags & (1 << esc_index)) == 0; // Set failure bits for this motor @@ -379,7 +379,7 @@ void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const const int all_escs_armed_mask = (1 << limited_esc_count) - 1; const bool is_all_escs_armed = (all_escs_armed_mask == esc_status.esc_armed_flags); - _esc_arm_hysteresis.set_hysteresis_time_from(false, ESC_TIMEOUT_US); + _esc_arm_hysteresis.set_hysteresis_time_from(false, ESC_ARMING_TIMEOUT_US); _esc_arm_hysteresis.set_state_and_update(!is_all_escs_armed, now); if (_esc_arm_hysteresis.get_state()) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp index 8989317b6e4..004b7d2baeb 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp @@ -52,6 +52,13 @@ public: uint16_t getMotorFailureMask() const { return _motor_failure_mask; } bool getEscArmStatus() const { return _esc_arm_hysteresis.get_state(); } + // An ESC whose last telemetry report is older than this is reported offline + static constexpr hrt_abstime ESC_OFFLINE_TIMEOUT_US = 400_ms; + + // Time an ESC may report disarmed while PX4 is armed, e.g. right after arming, before "Not all ESCs are armed" fires + // VertiQ ESCs over DroneCAN reliably report being armed after 800-890 ms + static constexpr hrt_abstime ESC_ARMING_TIMEOUT_US = 900_ms; + private: uint16_t checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now); uint16_t checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status); @@ -59,9 +66,6 @@ private: void updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now); void checkEscTemperature(Report &reporter, const esc_status_s &esc_status); - - static constexpr hrt_abstime ESC_TIMEOUT_US = 400_ms; - uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; diff --git a/src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp new file mode 100644 index 00000000000..893585506f7 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include "checks/escCheck.hpp" + +#include +#include +#include +#include +#include + +using namespace time_literals; + +// to run: make tests TESTFILTER=escChecks + +/* EVENT + * @skip-file + */ + +class EscChecksTest : public ::testing::Test +{ +public: + void SetUp() override + { + param_control_autosave(false); + + const int32_t one = 1; + param_set_no_notification(param_find("COM_ARM_CHK_ESCS"), &one); + param_set_no_notification(param_find("FD_ACT_EN"), &one); + } + + void TearDown() override + { + param_reset(param_find("COM_ARM_CHK_ESCS")); + param_reset(param_find("FD_ACT_EN")); + } + + // Publish a quad ESC status with every ESC fresh except `stale_index` whose last report is `age` old + void publishWithStaleEsc(int stale_index, hrt_abstime age, uint8_t armed_flags = 0b1111) + { + esc_status_s esc_status{}; + esc_status.timestamp = hrt_absolute_time(); + esc_status.esc_count = 4; + esc_status.esc_online_flags = 0b1111; // driver-side flags stay online, only the timestamp ages + esc_status.esc_armed_flags = armed_flags; + + for (int i = 0; i < 4; i++) { + esc_status.esc[i].timestamp = (i == stale_index) ? esc_status.timestamp - age : esc_status.timestamp; + esc_status.esc[i].actuator_function = esc_report_s::ACTUATOR_FUNCTION_MOTOR1 + i; + } + + _esc_status_pub.publish(esc_status); + } + + void runCheck(bool armed = false) + { + vehicle_status_s status{}; + + if (armed) { status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; } + + _check.updateParams(); + Context context{status}; + _failsafe_flags = {}; + Report reporter{_failsafe_flags, 0}; + _check.checkAndReport(context, reporter); + + _can_arm = reporter.armingCheckResults().can_arm == NavModes::All; + _health_error_escs = reporter.healthResults().error & health_component_t::motors_escs; + } + + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; + failsafe_flags_s _failsafe_flags{}; + bool _can_arm{true}; + bool _health_error_escs{false}; + EscChecks _check; +}; + +// An ESC report older than ESC_OFFLINE_TIMEOUT_US marks the ESC offline, blocks arming, and sets the motor failure mask +TEST_F(EscChecksTest, EscOfflineAfterOfflineTimeout) +{ + publishWithStaleEsc(2, EscChecks::ESC_OFFLINE_TIMEOUT_US + 50_ms); + runCheck(); + EXPECT_FALSE(_can_arm); + EXPECT_TRUE(_health_error_escs); + EXPECT_EQ(_check.getMotorFailureMask(), 0b0100u); + EXPECT_TRUE(_failsafe_flags.fd_motor_failure); +} + +TEST_F(EscChecksTest, EscOnlineInsideOfflineTimeout) +{ + publishWithStaleEsc(2, EscChecks::ESC_OFFLINE_TIMEOUT_US - 50_ms); + runCheck(); + EXPECT_TRUE(_can_arm); + EXPECT_FALSE(_health_error_escs); + EXPECT_EQ(_check.getMotorFailureMask(), 0u); + EXPECT_FALSE(_failsafe_flags.fd_motor_failure); +} + +// Right after arming the ESCs have not reported armed yet, which must not fail before the arming timeout +TEST_F(EscChecksTest, EscsNotYetArmedTolerated) +{ + static_assert(EscChecks::ESC_ARMING_TIMEOUT_US > EscChecks::ESC_OFFLINE_TIMEOUT_US, "arming timeout must outlast the offline timeout"); + publishWithStaleEsc(-1, 0, 0b0000); // all fresh, none armed yet + runCheck(true); + px4_usleep(EscChecks::ESC_ARMING_TIMEOUT_US - 50_ms); + publishWithStaleEsc(-1, 0, 0b0000); + runCheck(true); + EXPECT_FALSE(_failsafe_flags.fd_esc_arming_failure); + EXPECT_FALSE(_check.getEscArmStatus()); + EXPECT_EQ(_check.getMotorFailureMask(), 0u); +} + +// ESCs still not reporting armed once the arming timeout passed triggers the ESC arming failure +TEST_F(EscChecksTest, EscsNotArmedFailAfterArmingTimeout) +{ + publishWithStaleEsc(-1, 0, 0b0000); // all fresh, none armed yet + runCheck(true); + px4_usleep(EscChecks::ESC_ARMING_TIMEOUT_US + 50_ms); + publishWithStaleEsc(-1, 0, 0b1011); // ESC 3 still not armed + runCheck(true); + EXPECT_TRUE(_failsafe_flags.fd_esc_arming_failure); + EXPECT_TRUE(_check.getEscArmStatus()); + EXPECT_TRUE(_health_error_escs); +} + +// A stale ESC is ignored when COM_ARM_CHK_ESCS is disabled +TEST_F(EscChecksTest, EscOfflineIgnoredWithChecksDisabled) +{ + const int32_t zero = 0; + param_set_no_notification(param_find("COM_ARM_CHK_ESCS"), &zero); + publishWithStaleEsc(2, EscChecks::ESC_OFFLINE_TIMEOUT_US + 50_ms); + runCheck(); + EXPECT_TRUE(_can_arm); + EXPECT_FALSE(_health_error_escs); + EXPECT_EQ(_check.getMotorFailureMask(), 0u); + EXPECT_FALSE(_failsafe_flags.fd_motor_failure); +}