Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-10-06 09:02:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(commander): decouple ESC arming timeout from ESC telemetry timeout (#28915)

Browse Source 
* 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 <maetugr@gmail.com>
This commit is contained in:
Anil Kircaliali
2026-09-30 09:17:26 -07:00
committed by GitHub
co-authored by Matthias Grob
parent b07a720cfd
commit f20ec45f7f
4 changed files with 179 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
+1
View File
@@ -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)
src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
+2 -2
View File
@@ -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()) {
src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
+7 -3
View File
@@ -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)};
src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp
+169
View File
@@ -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 <gtest/gtest.h>
#include "checks/escCheck.hpp"
#include <drivers/drv_hrt.h>
#include <px4_platform_common/param.h>
#include <px4_platform_common/time.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/esc_status.h>
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_s> _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);
}