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); +}