From 9f978b05f324ab9c76aaccc3e34e6cf551e6d2d7 Mon Sep 17 00:00:00 2001 From: ttechnick Date: Fri, 30 Jan 2026 10:34:18 +0100 Subject: [PATCH] uavcan: unify timeout handling --- .../HealthAndArmingChecks/checks/escCheck.cpp | 3 +- .../failure_detector/FailureDetector.cpp | 156 +++++++++++------- .../failure_detector/FailureDetector.hpp | 2 + 3 files changed, 96 insertions(+), 65 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index dc8afc8bcf..8ffa938510 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -146,7 +146,8 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e 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::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; diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index cbba4c68e0..f7f3869a8d 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -72,14 +72,17 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic 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); } + } - if (_param_fd_act_en.get()) { - updateMotorStatus(vehicle_status, esc_status); - } + // Run motor status checks even when no new ESC data arrives (to detect timeouts) + if (_param_fd_act_en.get() && _esc_status_received) { + updateMotorStatus(vehicle_status, _last_esc_status); } if (_param_fd_imb_prop_thr.get() > 0) { @@ -188,11 +191,9 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c } _esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms); - _esc_failure_hysteresis.set_state_and_update(is_esc_failure, now); + _esc_failure_hysteresis.set_state_and_update(!is_all_escs_armed, now); - if (_esc_failure_hysteresis.get_state()) { - _failure_detector_status.flags.arm_escs = true; - } + _failure_detector_status.flags.arm_escs = _esc_failure_hysteresis.get_state(); } else { // reset ESC bitfield @@ -266,74 +267,101 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, // 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; - // Only check while armed - if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - actuator_motors_s actuator_motors{}; - _actuator_motors_sub.copy(&actuator_motors); + // Clear the failure mask at the start --> Can recover when issue is resolved!! + _motor_failure_mask = 0; - // 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 - } + // Check telemetry timeout always, current checks only when armed + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); - const bool timeout = now > esc_status.esc[i].timestamp + 300_ms; - const float current = esc_status.esc[i].esc_current; + // Check individual ESC reports + for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) { + const bool mapped = math::isInRange(esc_status.esc[i].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1, + uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1)); - // 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; - } - - _motor_failure_mask &= ~(1u << actuator_function_index); // Reset bit in mask to accumulate failures again - _motor_failure_mask |= (static_cast(timeout) << actuator_function_index); // Telemetry timeout - - // 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); + // Skip if ESC is not actually connected + if (!mapped) { + continue; } - _failure_detector_status.flags.motor = (_motor_failure_mask != 0u); + // 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; - } else { // Disarmed + + 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; + + // 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); + } + + + // 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; + } + + 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 because a reaction could be to stop the motor and that would be conidered healty 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 because a reaction could be to stop the motor and that would be conidered healty 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 = false; } + + + _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 26182f4c3c..abaeb5c294 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -115,6 +115,8 @@ private: 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