diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 8ffa938510..79cc209e71 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -134,7 +134,11 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) { - if (esc_status.esc[index].failures != 0) { + const bool esc_failure = (esc_status.esc[index].failures != 0); + reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR; + const bool motor_failure = (reporter.failsafeFlags().fd_motor_failure); + + if (esc_failure) { 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)) { @@ -178,6 +182,39 @@ 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; + + 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); + + 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); + } + } + } + } + } + } } + + } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp index a96c147234..32a726e14b 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp @@ -37,6 +37,7 @@ #include #include +#include class EscChecks : public HealthAndArmingCheckBase { @@ -50,6 +51,7 @@ private: void checkEscStatus(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)}; const hrt_abstime _start_time{hrt_absolute_time()}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp index fed77a0b41..fc618f1a26 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp @@ -139,21 +139,6 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor } } - reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR; - if (reporter.failsafeFlags().fd_motor_failure) { - /* 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 failure detected"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor failure detected"); - } - } } diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index f7f3869a8d..329cc2d876 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -262,6 +262,13 @@ 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 @@ -269,7 +276,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, 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!! + // Clear the failure mask at the start --> Can recover when issue is resolved! _motor_failure_mask = 0; @@ -299,6 +306,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, 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) { @@ -309,6 +317,10 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, _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) {