From 2a3719006fb267dbbc63ded9d4094313f9fd45e3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Math=C3=A9o=20Taillandier?= <123255318+Matheo-13@users.noreply.github.com> Date: Fri, 20 Nov 2026 19:28:25 +0100 Subject: [PATCH] feat(commander): when disarmed, do not report ESC offline on battery blocking preflight checks (#29523) * feat(commander): when disarmed, do not report ESC offline on battery blocking preflight checks * fix(commander): keep the ESC checks with the supply check circuit breaker set With CBRK_SUPPLY_CHK set the battery check returns without reporting a missing required battery, so skipping the ESC checks let a vehicle arm with that battery missing and its ESCs unpowered. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Matheo-13 * fix(commander): clear the ESC failure state while a required battery is missing The skip only cleared the motor failure mask, leaving fd_motor_failure, fd_esc_arming_failure and the ESC arming hysteresis at their last values: after a battery dropped in flight and the vehicle disarmed, failsafe_flags and failure_detector_status disagreed until the battery reconnected. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Matheo-13 * feat(commander): give the ESCs time to boot once a required battery connects Once the battery reconnected the checks resumed against ESC timestamps from before it was unplugged, so "ESC N offline" still showed while the ESCs booted. Reuse the boot grace already given to ESC telemetry at startup. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Matheo-13 * perf(commander): only read the batteries in the ESC checks when disarmed The battery skip only applies on ground, so the armed path no longer copies every battery instance each cycle. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Matheo-13 * test(commander): cover the ESC checks with a required battery missing Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Matheo-13 * fix(commander): block arming while the ESCs boot after a required battery connects * test(commander): keep the failsafe flags across ESC check runs Commander keeps the failsafe flags across check cycles, so zeroing them before every run hid any flag the ESC checks forget to clear. Cover the skip while a required battery is missing clearing a motor failure that was latched in flight. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Matheo-13 * fix(commander): clear the motor stuck flag while a required battery is missing The skip for a missing required battery returned before the motor stuck flag was cleared, so a stuck motor flagged at spool-up stayed set on the ground, unlike the other failure flags the skip clears. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Matheo-13 --------- Signed-off-by: Matheo-13 --- .../HealthAndArmingChecks/checks/escCheck.cpp | 53 ++++++++- .../HealthAndArmingChecks/checks/escCheck.hpp | 10 ++ .../HealthAndArmingChecks/escChecksTest.cpp | 107 +++++++++++++++++- 3 files changed, 164 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 1d9b776f6d2..331c01e787e 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "escCheck.hpp" +#include #include #include @@ -71,10 +72,58 @@ static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_r void EscChecks::checkAndReport(const Context &context, Report &reporter) { const hrt_abstime now = hrt_absolute_time(); - esc_status_s esc_status; reporter.failsafeFlags().fd_motor_stuck_failure = false; + // On ground a missing required battery is reported and blocks arming: skip the ESC checks, its ESCs are unpowered, + // and give them ESC_INIT_TIME_US to boot once it connects, during which arming stays blocked. + // CBRK_SUPPLY_CHK disables that report, so the ESC checks then stay the only ones to block arming. + if (!context.isArmed() && !circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) { + bool required_battery_missing = false; + + for (auto &battery_sub : _battery_status_subs) { + battery_status_s battery; + + if (battery_sub.copy(&battery) && battery.is_required && !battery.connected) { + required_battery_missing = true; + } + } + + if (_required_battery_missing && !required_battery_missing) { + _escs_init_end = now + ESC_INIT_TIME_US; + } + + _required_battery_missing = required_battery_missing; + + if (required_battery_missing || now < _escs_init_end) { + // Clear what the disarmed checks clear, a failure from before the battery dropped would otherwise stay set + _motor_failure_mask = 0; + _motor_failure_detector.reset(); + _esc_arm_hysteresis.set_state_and_update(false, now); + reporter.failsafeFlags().fd_motor_failure = false; + reporter.failsafeFlags().fd_esc_arming_failure = false; + + // The battery check no longer blocks arming once the battery connects: block it until the ESCs booted + if (!required_battery_missing && _param_com_arm_chk_escs.get() > 0) { + /* EVENT + * @description + * The ESCs get time to boot once a required battery connects. + * + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::motors_escs, + events::ID("check_escs_initializing"), + events::Log::Info, "Waiting for ESCs to initialize"); + } + + return; + } + } + + esc_status_s esc_status; + // Run motor status checks even when no new ESC data arrives (to detect timeouts) if (_esc_status_sub.copy(&esc_status)) { if (esc_status.esc_count <= 0) { @@ -105,7 +154,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter) reporter.setIsPresent(health_component_t::motors_escs); reporter.failsafeFlags().fd_motor_failure = (mask != 0); - } else if ((_param_com_arm_chk_escs.get() > 0) && now > _start_time + 3_s) { // Allow ESCs to init + } else if ((_param_com_arm_chk_escs.get() > 0) && now > _start_time + ESC_INIT_TIME_US) { // Allow ESCs to init /* EVENT * @description * diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp index 888cf7d78cd..11e0f7021ac 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp @@ -38,7 +38,9 @@ #include #include #include +#include #include +#include #include #include @@ -57,6 +59,9 @@ public: // An ESC whose last telemetry report is older than this is reported offline static constexpr hrt_abstime ESC_OFFLINE_TIMEOUT_US = 400_ms; + // Time the ESCs get to boot and report telemetry once powered + static constexpr hrt_abstime ESC_INIT_TIME_US = 3_s; + // 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; @@ -84,9 +89,13 @@ private: uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; const hrt_abstime _start_time{hrt_absolute_time()}; + bool _required_battery_missing{false}; + hrt_abstime _escs_init_end{0}; // end of the boot time of ESCs powered by a required battery that just connected + uint16_t _motor_failure_mask = 0; bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; // true if ESC reported non-zero current before (some never report any) bool _esc_over_temp_warned{false}; @@ -96,6 +105,7 @@ private: DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamBool) _param_com_arm_chk_escs, + (ParamInt) _param_cbrk_supply_chk, (ParamBool) _param_fd_act_en, (ParamFloat) _param_com_spoolup_time, (ParamFloat) _param_motfail_c2t, diff --git a/src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp index 118978cd62e..b029e671cf5 100644 --- a/src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/escChecksTest.cpp @@ -39,10 +39,12 @@ #include "checks/escCheck.hpp" #include +#include #include #include #include #include +#include #include using namespace time_literals; @@ -75,6 +77,18 @@ public: param_reset(param_find("FD_ACT_EN")); param_reset(param_find("COM_SPOOLUP_TIME")); param_reset(param_find("MOTFAIL_UND_TIME")); + param_reset(param_find("CBRK_SUPPLY_CHK")); + + _battery_status_pub.publish(battery_status_s{}); // the topic outlives the test: leave no required battery behind + } + + void publishBattery(bool required, bool connected) + { + battery_status_s battery_status{}; + battery_status.timestamp = hrt_absolute_time(); + battery_status.is_required = required; + battery_status.connected = connected; + _battery_status_pub.publish(battery_status); } // Publish a quad ESC status with every ESC fresh except `stale_index` whose last report is `age` old @@ -140,8 +154,7 @@ public: _check.updateParams(); Context context{status}; - _failsafe_flags = {}; - Report reporter{_failsafe_flags, 0}; + Report reporter{_failsafe_flags, 0}; // the flags persist across runs, as in commander: a check must clear what it set _check.checkAndReport(context, reporter); _can_arm = reporter.armingCheckResults().can_arm == NavModes::All; @@ -190,8 +203,7 @@ public: _check.updateParams(); Context context{status}; - _failsafe_flags = {}; - Report reporter{_failsafe_flags, 0}; + Report reporter{_failsafe_flags, 0}; // the flags persist across runs, as in commander _check.checkAndReport(context, reporter); _health_error_escs = reporter.healthResults().error & health_component_t::motors_escs; @@ -205,6 +217,7 @@ public: uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _battery_status_pub{ORB_ID(battery_status)}; hrt_abstime _esc_time{0}; // simulated ESC sample time, starts at the real time of the first fly() failsafe_flags_s _failsafe_flags{}; bool _can_arm{true}; @@ -459,3 +472,89 @@ TEST_F(EscChecksTest, EscSharingMotorFunctionJudgedSeparately) EXPECT_EQ(_check.getMotorFailureMask(), 0b0100u); EXPECT_TRUE(_failsafe_flags.fd_motor_failure); } + +// On ground a missing required battery blocks arming on its own: its unpowered ESCs are not reported offline +TEST_F(EscChecksTest, EscOfflineIgnoredDisarmedWithRequiredBatteryMissing) +{ + publishBattery(true, false); + 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); +} + +TEST_F(EscChecksTest, EscOfflineReportedArmedWithRequiredBatteryMissing) +{ + publishBattery(true, false); + publishWithStaleEsc(2, EscChecks::ESC_OFFLINE_TIMEOUT_US + 50_ms); + runCheck(true); + EXPECT_TRUE(_health_error_escs); + EXPECT_EQ(_check.getMotorFailureMask(), 0b0100u); + EXPECT_TRUE(_failsafe_flags.fd_motor_failure); +} + +// A motor failure latched in flight is cleared on disarm also when the required battery is missing, +// where the ESC checks are skipped +TEST_F(EscChecksTest, MotorFailureClearedDisarmedWithRequiredBatteryMissing) +{ + fly(200_ms, -1); + fly(600_ms, 2); + ASSERT_EQ(_check.getMotorFailureMask(), 0b0100u); + ASSERT_TRUE(_failsafe_flags.fd_motor_failure); + + publishBattery(true, false); + runCheck(false); + EXPECT_EQ(_check.getMotorFailureMask(), 0u); + EXPECT_FALSE(_failsafe_flags.fd_motor_failure); + EXPECT_FALSE(_failsafe_flags.fd_esc_arming_failure); + EXPECT_FALSE(_health_error_escs); +} + +// A stuck motor flagged at spool-up is cleared on disarm also when the required battery is missing +TEST_F(EscChecksTest, MotorStuckClearedDisarmedWithRequiredBatteryMissing) +{ + publishSpinup({2000, 0, 2000, 2000}, kIdle); + runSpinupCheck(hrt_absolute_time() - kAtDecision); + ASSERT_TRUE(_failsafe_flags.fd_motor_stuck_failure); + + publishBattery(true, false); + runCheck(false); + EXPECT_FALSE(_failsafe_flags.fd_motor_stuck_failure); +} + +// CBRK_SUPPLY_CHK stops the battery check from reporting the missing battery, so the ESC checks must still block arming +TEST_F(EscChecksTest, EscOfflineReportedWithSupplyCheckBreaker) +{ + const int32_t cbrk_supply_chk = CBRK_SUPPLY_CHK_KEY; + param_set_no_notification(param_find("CBRK_SUPPLY_CHK"), &cbrk_supply_chk); + publishBattery(true, false); + publishWithStaleEsc(2, EscChecks::ESC_OFFLINE_TIMEOUT_US + 50_ms); + runCheck(); + EXPECT_FALSE(_can_arm); + EXPECT_TRUE(_health_error_escs); + EXPECT_EQ(_check.getMotorFailureMask(), 0b0100u); +} + +// Once the required battery connects its ESCs get ESC_INIT_TIME_US to boot before they are reported offline, +// arming stays blocked meanwhile +TEST_F(EscChecksTest, EscsGivenTimeToBootOnceRequiredBatteryConnects) +{ + publishBattery(true, false); + publishWithStaleEsc(2, EscChecks::ESC_OFFLINE_TIMEOUT_US + 50_ms); + runCheck(); + + publishBattery(true, true); + runCheck(); + EXPECT_FALSE(_can_arm); + EXPECT_FALSE(_health_error_escs); + EXPECT_EQ(_check.getMotorFailureMask(), 0u); + + px4_usleep(EscChecks::ESC_INIT_TIME_US + 50_ms); + publishWithStaleEsc(2, EscChecks::ESC_OFFLINE_TIMEOUT_US + 50_ms); + runCheck(); + EXPECT_FALSE(_can_arm); + EXPECT_TRUE(_health_error_escs); + EXPECT_EQ(_check.getMotorFailureMask(), 0b0100u); +}