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 <matheo.taillandier@gmail.com>

* 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 <matheo.taillandier@gmail.com>

* 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 <matheo.taillandier@gmail.com>

* 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 <matheo.taillandier@gmail.com>

* test(commander): cover the ESC checks with a required battery missing

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Matheo-13 <matheo.taillandier@gmail.com>

* 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 <matheo.taillandier@gmail.com>

* 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 <matheo.taillandier@gmail.com>

---------

Signed-off-by: Matheo-13 <matheo.taillandier@gmail.com>
This commit is contained in:
Mathéo Taillandier authored and GitHub committed 2026-11-20 11:28:25 -07:00
1 parent 1687722479
commit 2a3719006f
3 files changed
+164 -6

No files matched your search

@@ -32,6 +32,7 @@
****************************************************************************/
#include "escCheck.hpp"
#include <lib/circuit_breaker/circuit_breaker.h>
#include <px4_platform_common/events.h>
#include <uORB/topics/actuator_motors.h>
@@ -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.
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
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
* <profile name="dev">
@@ -38,7 +38,9 @@
#include <lib/hysteresis/hysteresis.h>
#include <lib/motor_failure_detector/MotorFailureDetector.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/failure_detector_status.h>
@@ -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_s, battery_status_s::MAX_INSTANCES> _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<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs,
(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
(ParamBool<px4::params::FD_ACT_EN>) _param_fd_act_en,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
(ParamFloat<px4::params::MOTFAIL_C2T>) _param_motfail_c2t,
@@ -39,10 +39,12 @@
#include "checks/escCheck.hpp"
#include <drivers/drv_hrt.h>
#include <lib/circuit_breaker/circuit_breaker.h>
#include <px4_platform_common/param.h>
#include <px4_platform_common/time.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
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_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<battery_status_s> _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);
}