From 96c5c7ba024df263d0aa7e7985329f86ddc5cfc5 Mon Sep 17 00:00:00 2001 From: ttechnick Date: Tue, 10 Feb 2026 18:35:47 +0100 Subject: [PATCH] work on: feed back checks to commander --- src/modules/commander/Commander.hpp | 2 +- .../HealthAndArmingChecks.hpp | 3 + .../HealthAndArmingChecks/checks/escCheck.cpp | 309 +++++++++++++++--- .../HealthAndArmingChecks/checks/escCheck.hpp | 31 +- .../checks/failureDetectorCheck.cpp | 20 -- .../failure_detector/FailureDetector.cpp | 168 +--------- .../failure_detector/FailureDetector.hpp | 16 +- 7 files changed, 300 insertions(+), 249 deletions(-) diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index ff6ed018be..6a989d9f96 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -224,7 +224,7 @@ private: Failsafe _failsafe_instance{this}; FailsafeBase &_failsafe{_failsafe_instance}; - FailureDetector _failure_detector{this}; + FailureDetector _failure_detector{this, _health_and_arming_checks}; HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; MulticopterThrowLaunch _multicopter_throw_launch{this}; Safety _safety{}; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index 1986e55e15..53e9a5a534 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -109,6 +109,9 @@ public: const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; } + uint16_t getMotorFailureMask() const {return _esc_checks.getMotorFailureMask(); } + bool getEscArmStatus() const { return _esc_checks.getEscArmStatus(); } + #ifndef CONSTRAINED_FLASH ExternalChecks &externalChecks() { return _external_checks; } #endif diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 92a193593c..1da42e4f0e 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -73,17 +73,30 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter) { const hrt_abstime now = hrt_absolute_time(); const hrt_abstime esc_telemetry_timeout = - 700_ms; // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that + 2_s; // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that esc_status_s esc_status; - if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + esc_telemetry_timeout)) { - checkEscStatus(context, reporter, 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) { + return; + } + uint16_t mask = 0; + + mask |= checkEscOnline(context, reporter, esc_status); + mask |= checkEscStatus(context, reporter, esc_status); + updateEscsStatus(context, reporter, esc_status); + + if (!_param_fd_act_en.get()) { + mask |= checkMotorStatus(context, reporter, esc_status); + } + _motor_failure_mask = mask; reporter.setIsPresent(health_component_t::motors_escs); } else if (_param_com_arm_chk_escs.get() - && now - _start_time > 5_s) { // Wait a bit after startup to allow esc's to init + && now - _start_time > esc_telemetry_timeout) { // Wait a bit after startup to allow esc's to init /* EVENT * @description @@ -100,42 +113,217 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter) } } -void EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status) +uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status) { - const NavModes required_modes = _param_com_arm_chk_escs.get() ? NavModes::All : NavModes::None; - - if (esc_status.esc_count <= 0) { - return; + // Check if one or more the ESCs are offline + if (!_param_com_arm_chk_escs.get()){ + return 0; } - + uint16_t mask = 0; char esc_fail_msg[50]; esc_fail_msg[0] = '\0'; - int online_bitmask = (1 << esc_status.esc_count) - 1; + for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) { - // Check if one or more the ESCs are offline - if (online_bitmask != esc_status.esc_online_flags) { + // check if mapped + if (!math::isInRange(esc_status.esc[index].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1, + uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) { + continue; // Skip unmapped ESC status entries + } + const uint8_t actuator_function_index = + esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1; - for (int index = 0; index < esc_status.esc_count; index++) { - if ((esc_status.esc_online_flags & (1 << index)) == 0) { - uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1; + const bool timeout = hrt_absolute_time() > esc_status.esc[index].timestamp + 300_ms; + const bool is_offline = (esc_status.esc_online_flags & (1 << index)) == 0; + + + + // Set failure bits for this motor + if (timeout || is_offline) { + mask |= (1u << actuator_function_index); + + uint8_t motor_index = actuator_function_index + 1; + /* EVENT + * @description + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_escs_offline"), + events::Log::Critical, "ESC {1} offline", motor_index); + snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", motor_index); + esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; + } + } + + if (reporter.mavlink_log_pub() && esc_fail_msg[0] != '\0') { + mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : ""); + + } + return mask; + +} + +uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status) +{ + if (!_param_com_arm_chk_escs.get()){ + return 0; + } + + uint16_t mask = 0; + + for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) { + + if (!math::isInRange(esc_status.esc[index].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1, + uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) { + continue; // Skip unmapped ESC status entries + } + + const uint8_t act_function_index = + esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1; + + if (esc_status.esc[index].failures == 0) { + continue; + } + else + { + mask |= (1u << act_function_index); // Set bit in mask + } + + + for (uint8_t fault_index = 0; fault_index <= static_cast(esc_fault_reason_t::_max); fault_index++) { + if (!(esc_status.esc[index].failures & (1 << fault_index))) { + continue; + } + + esc_fault_reason_t fault_reason_index = static_cast(fault_index); + + const char *user_action = ""; + events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none; + + if (context.isArmed()) { + if (fault_reason_index == esc_fault_reason_t::motor_warn_temp + || fault_reason_index == esc_fault_reason_t::esc_warn_temp + || fault_reason_index == esc_fault_reason_t::over_rpm) { + user_action = "Reduce throttle"; + action = events::px4::enums::suggested_action_t::reduce_throttle; + + } else { + user_action = "Land now!"; + action = events::px4::enums::suggested_action_t::land; + } + } + + /* EVENT + * @description + * {3} + * + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.healthFailure( + NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"), + events::Log::Critical, "ESC {1}: {2}", act_function_index + 1, fault_reason_index, action); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", act_function_index + 1, + esc_fault_reason_str(fault_reason_index), user_action); + } + } + } + return mask; +} + +uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status) +{ + if (!_param_fd_act_en.get()){ + return 0; + } + const hrt_abstime now = hrt_absolute_time(); + uint16_t mask = 0; + + // Only check while armed + if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + + // Check individual ESC reports + for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) { + // Map the esc status index to the actuator function index + const uint8_t actuator_function_index = + esc_status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1; + + if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) { + continue; // Invalid mapping + } + + const float current = esc_status.esc[i].esc_current; + + // First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it. + if (current > FLT_EPSILON) { + _esc_has_reported_current[i] = true; + } + + if (!_esc_has_reported_current[i]) { + continue; + } + + // Current limits + float thrust = 0.f; + + if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) { + // Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust + thrust = fabsf(actuator_motors.control[actuator_function_index]); + } + + bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get(); + bool current_too_low = current < (thrust * _param_fd_act_mot_c2t.get()) - _param_fd_act_low_off.get(); + bool current_too_high = current > (thrust * _param_fd_act_mot_c2t.get()) + _param_fd_act_high_off.get(); + + _esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms); + _esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms); + + if (!_esc_undercurrent_hysteresis[i].get_state()) { + // Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again + _esc_undercurrent_hysteresis[i].set_state_and_update(thrust_above_threshold && current_too_low, now); + } + + if (!_esc_overcurrent_hysteresis[i].get_state()) { + // Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again + _esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high, now); + } + + mask |= (static_cast(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index); + mask |= (static_cast(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index); + + if (_esc_undercurrent_hysteresis[i].get_state()) { /* EVENT * @description * - * This check can be configured via COM_ARM_CHK_ESCS parameter. + * This check can be configured via FD_ACT_EN parameter. * */ - reporter.healthFailure(required_modes, health_component_t::motors_escs, events::ID("check_escs_offline"), - events::Log::Critical, "ESC {1} offline", motor_index); - snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", motor_index); - esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; - } - } + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, + events::ID("check_failure_detector_motor_uc"), + events::Log::Critical, "Motor {1} undercurrent detected", actuator_function_index + 1); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : ""); - } - } + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Motor failure: Motor %d undercurrent detected", + actuator_function_index + 1); + } + } + + if (_esc_overcurrent_hysteresis[i].get_state()) { + /* EVENT + * @description + * + * This check can be configured via FD_ACT_EN parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, + events::ID("check_failure_detector_motor_oc"), + events::Log::Critical, "Motor {1} overcurrent detected", actuator_function_index + 1); for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) { @@ -188,36 +376,51 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e } } - if (motor_failure) { - reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR; + } else { // Disarmed + for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) { + _esc_undercurrent_hysteresis[i].set_state_and_update(false, now); + _esc_overcurrent_hysteresis[i].set_state_and_update(false, now); + } - if (reporter.failsafeFlags().fd_motor_failure) { - // Get the failure detector status to check which motor(s) failed - failure_detector_status_s failure_detector_status{}; - bool have_motor_mask = _failure_detector_status_sub.copy(&failure_detector_status); + } + return mask; +} - if (have_motor_mask && failure_detector_status.motor_failure_mask != 0) { - for (uint8_t motor_index = 0; motor_index < esc_status_s::CONNECTED_ESC_MAX; motor_index++) { - if (failure_detector_status.motor_failure_mask & (1 << motor_index)) { - /* EVENT - * @description - * - * This check can be configured via FD_ACT_EN parameter. - * - */ - reporter.healthFailure(NavModes::All, health_component_t::motors_escs, - events::ID("check_failure_detector_motor"), - events::Log::Critical, "Motor {1} failure detected", motor_index); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor %d failure detected", - motor_index); - } - } - } - } +void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status) +{ + if (!_param_com_arm_chk_escs.get()){ + return; + } + hrt_abstime now = hrt_absolute_time(); + + if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX); + 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, 300_ms); + _esc_arm_hysteresis.set_state_and_update(!is_all_escs_armed, now); + + if (_esc_arm_hysteresis.get_state()) { + /* EVENT + * @description + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, + events::ID("check_escs_not_all_armed"), + events::Log::Critical, "Not all ESCs are armed"); + + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "ESC failure: Not all ESCs are armed. Land now!"); } } - } + } else { + // reset ESC bitfield + _esc_arm_hysteresis.set_state_and_update(false, now); + } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp index 32a726e14b..20e1cf15c2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp @@ -35,7 +35,9 @@ #include "../Common.hpp" +#include #include +#include #include #include @@ -47,15 +49,38 @@ public: void checkAndReport(const Context &context, Report &reporter) override; + uint16_t getMotorFailureMask() const { + return _motor_failure_mask; } + bool getEscArmStatus() const { return _esc_arm_hysteresis.get_state(); } + private: - void checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status); + uint16_t checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status); + uint16_t checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status); + uint16_t checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status); + void updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status); + uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; const hrt_abstime _start_time{hrt_absolute_time()}; + uint16_t _motor_failure_mask{0}; + esc_status_s _last_esc_status{}; + bool _esc_status_received{false}; + bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; + systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX]; + systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX]; + systemlib::Hysteresis _esc_arm_hysteresis; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamBool) _param_com_arm_chk_escs - ) + (ParamBool) _param_com_arm_chk_escs, + (ParamBool) _param_fd_act_en, + (ParamFloat) _param_fd_act_mot_thr, + (ParamFloat) _param_fd_act_mot_c2t, + (ParamInt) _param_fd_act_mot_tout, + (ParamFloat) _param_fd_act_low_off, + (ParamFloat) _param_fd_act_high_off); }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp index fc618f1a26..48c2dbff28 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp @@ -99,26 +99,6 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor vehicle_status_s::FAILURE_EXT); - reporter.failsafeFlags().fd_esc_arming_failure = context.status().failure_detector_status & - vehicle_status_s::FAILURE_ARM_ESC; - - if (reporter.failsafeFlags().fd_esc_arming_failure) { - /* EVENT - * @description - * One or more ESCs failed to arm. - * - * - * This check can be configured via FD_ESCS_EN parameter. - * - */ - reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"), - events::Log::Critical, "ESC failure"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected"); - } - } - reporter.failsafeFlags().fd_imbalanced_prop = context.status().failure_detector_status & vehicle_status_s::FAILURE_IMBALANCED_PROP; diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 18446d78e4..da82194243 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -39,11 +39,13 @@ */ #include "FailureDetector.hpp" +#include "../HealthAndArmingChecks/HealthAndArmingChecks.hpp" using namespace time_literals; -FailureDetector::FailureDetector(ModuleParams *parent) : - ModuleParams(parent) +FailureDetector::FailureDetector(ModuleParams *parent, HealthAndArmingChecks &health_and_arming_checks) : + ModuleParams(parent), + _health_and_arming_checks(health_and_arming_checks) { } @@ -67,24 +69,6 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic _failure_detector_status.flags.ext = false; } - // esc_status subscriber is shared between subroutines - esc_status_s esc_status; - - if (_esc_status_sub.update(&esc_status)) { - _failure_injector.manipulateEscStatus(esc_status); - _last_esc_status = esc_status; - _esc_status_received = true; - - if (_param_escs_en.get()) { - updateEscsStatus(vehicle_status, esc_status); - } - } - - // Run motor status checks even when no new ESC data arrives (to detect timeouts) - if (_esc_status_received) { - updateMotorStatus(vehicle_status, _last_esc_status); - } - if (_param_fd_imb_prop_thr.get() > 0) { updateImbalancedPropStatus(); } @@ -99,12 +83,12 @@ void FailureDetector::publishStatus() failure_detector_status.fd_pitch = _failure_detector_status.flags.pitch; failure_detector_status.fd_alt = _failure_detector_status.flags.alt; failure_detector_status.fd_ext = _failure_detector_status.flags.ext; - failure_detector_status.fd_arm_escs = _failure_detector_status.flags.arm_escs; + failure_detector_status.fd_arm_escs = _health_and_arming_checks.getEscArmStatus(); failure_detector_status.fd_battery = _failure_detector_status.flags.battery; failure_detector_status.fd_imbalanced_prop = _failure_detector_status.flags.imbalanced_prop; - failure_detector_status.fd_motor = _failure_detector_status.flags.motor; + failure_detector_status.fd_motor = (_health_and_arming_checks.getMotorFailureMask() != 0) || (_failure_injector.getMotorStopMask() != 0); failure_detector_status.imbalanced_prop_metric = _imbalanced_prop_lpf.getState(); - failure_detector_status.motor_failure_mask = _motor_failure_mask; + failure_detector_status.motor_failure_mask = _health_and_arming_checks.getMotorFailureMask(); failure_detector_status.motor_stop_mask = _failure_injector.getMotorStopMask(); failure_detector_status.timestamp = hrt_absolute_time(); _failure_detector_status_pub.publish(failure_detector_status); @@ -175,32 +159,6 @@ void FailureDetector::updateExternalAtsStatus() } } -void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status) -{ - hrt_abstime now = hrt_absolute_time(); - - if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX); - 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); - - bool is_esc_failure = !is_all_escs_armed; - - for (int i = 0; i < limited_esc_count; i++) { - is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0); - } - - _esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms); - _esc_failure_hysteresis.set_state_and_update(!is_all_escs_armed, time_now); - - _failure_detector_status.flags.arm_escs = _esc_failure_hysteresis.get_state(); - - } else { - // reset ESC bitfield - _esc_failure_hysteresis.set_state_and_update(false, now); - _failure_detector_status.flags.arm_escs = false; - } -} void FailureDetector::updateImbalancedPropStatus() { @@ -260,115 +218,3 @@ void FailureDetector::updateImbalancedPropStatus() } } -void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status) -{ - // This check can be configured via FD_ACT_EN parameter. - - if (!_param_fd_act_en.get()) { - _failure_detector_status.flags.motor = false; - return; - } - - // 1. Telemetry times out -> communication or power lost on that ESC - // 2. Too low current draw compared to commanded thrust - // Overvoltage, overcurrent do not have checks yet esc_report.failures are handled separately - - const hrt_abstime now = hrt_absolute_time(); - const bool is_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; - - // Clear the failure mask at the start --> Can recover when issue is resolved! - _motor_failure_mask = 0; - - - // Check telemetry timeout always, current checks only when armed - actuator_motors_s actuator_motors{}; - _actuator_motors_sub.copy(&actuator_motors); - - // Check individual ESC reports - for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) { - const bool mapped = math::isInRange((int)esc_status.esc[i].actuator_function, (int)OutputFunction::Motor1, - (int)OutputFunction::MotorMax); - - // Skip if ESC is not actually connected - if (!mapped) { - continue; - } - - // Map the esc status index to the actuator function index - const uint8_t actuator_function_index = - esc_status.esc[i].actuator_function - (int)OutputFunction::Motor1; - - if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) { - continue; // Invalid mapping - } - - const bool timeout = now > esc_status.esc[i].timestamp + 300_ms; - const bool is_offline = (esc_status.esc_online_flags & (1 << i)) == 0; - const float current = esc_status.esc[i].esc_current; - const bool esc_flag = esc_status.esc[i].failures != 0; - - // Set failure bits for this motor - if (timeout) { - _motor_failure_mask |= (1u << actuator_function_index); - } - - if (is_offline) { - _motor_failure_mask |= (1u << actuator_function_index); - } - - if (esc_flag) { - _motor_failure_mask |= (1u << actuator_function_index); - } - - - // Current checks only when armed - if (!is_armed) { - continue; - } - - // First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it. - if (current > FLT_EPSILON) { - _esc_has_reported_current[i] = true; - } - - // Current limits - float thrust = 0.f; - - if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) { - // Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust - thrust = fabsf(actuator_motors.control[actuator_function_index]); - } - - bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get(); - bool current_too_low = current < (thrust * _param_fd_act_mot_c2t.get()) - _param_fd_act_low_off.get(); - bool current_too_high = current > (thrust * _param_fd_act_mot_c2t.get()) + _param_fd_act_high_off.get(); - - _esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms); - _esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms); - - if (!_esc_undercurrent_hysteresis[i].get_state()) { - // Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again - _esc_undercurrent_hysteresis[i].set_state_and_update(thrust_above_threshold && current_too_low && !timeout, now); - } - - if (!_esc_overcurrent_hysteresis[i].get_state()) { - // Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again - _esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high && !timeout, now); - } - - _motor_failure_mask |= (static_cast(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index); - _motor_failure_mask |= (static_cast(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index); - } - - - // Clear current-related hysteresis when disarmed - if (!is_armed) { - for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) { - _esc_undercurrent_hysteresis[i].set_state_and_update(false, now); - _esc_overcurrent_hysteresis[i].set_state_and_update(false, now); - } - } - - - _failure_detector_status.flags.motor = (_motor_failure_mask != 0u); -} diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index b84a17949d..d29691d0bf 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -81,10 +81,12 @@ union failure_detector_status_u { using uORB::SubscriptionData; +class HealthAndArmingChecks; + class FailureDetector : public ModuleParams { public: - FailureDetector(ModuleParams *parent); + FailureDetector(ModuleParams *parent, HealthAndArmingChecks &health_and_arming_checks); ~FailureDetector() = default; bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode); @@ -95,29 +97,21 @@ public: private: void updateAttitudeStatus(const vehicle_status_s &vehicle_status); void updateExternalAtsStatus(); - void updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status); - void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status); void updateImbalancedPropStatus(); failure_detector_status_u _failure_detector_status{}; + HealthAndArmingChecks &_health_and_arming_checks; + systemlib::Hysteresis _roll_failure_hysteresis{false}; systemlib::Hysteresis _pitch_failure_hysteresis{false}; systemlib::Hysteresis _ext_ats_failure_hysteresis{false}; - systemlib::Hysteresis _esc_failure_hysteresis{false}; static constexpr float _imbalanced_prop_lpf_time_constant{5.f}; AlphaFilter _imbalanced_prop_lpf{}; uint32_t _selected_accel_device_id{0}; hrt_abstime _imu_status_timestamp_prev{0}; - // Motor failure check - bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; // true if ESC reported non-zero current before (some never report any) - systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX]; - systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX]; - uint16_t _motor_failure_mask = 0; // actuator function indexed - esc_status_s _last_esc_status{}; // Store last received ESC status - bool _esc_status_received{false}; // Track if we've ever received ESC data uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance