diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index ee62ade96a..7b74a1b869 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -101,7 +101,7 @@ void FailureDetector::publishStatus() 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.imbalanced_prop_metric = _imbalanced_prop_lpf.getState(); - failure_detector_status.motor_failure_mask = _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; + failure_detector_status.motor_failure_mask = _motor_failure_mask; failure_detector_status.motor_stop_mask = _failure_injector.getMotorStopMask(); failure_detector_status.timestamp = hrt_absolute_time(); _failure_detector_status_pub.publish(failure_detector_status); @@ -261,110 +261,72 @@ void FailureDetector::updateImbalancedPropStatus() void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status) { - // What need to be checked: - // - // 1. ESC telemetry disappears completely -> dead ESC or power loss on that ESC - // 2. ESC failures like overvoltage, overcurrent etc. But DShot driver for example is not populating the field 'esc_report.failures' - // 3. Motor current too low. Compare drawn motor current to expected value from a parameter - // -- ESC voltage does not really make sense and is highly dependent on the setup + // 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 - // First wait for some ESC telemetry that has the required fields. Before that happens, don't check this ESC - // Then check + const hrt_abstime now = hrt_absolute_time(); // Only check while armed if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - const hrt_abstime now = hrt_absolute_time(); - const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX); - - actuator_motors_s actuator_motors{}; + actuator_motors_s actuator_motors{}; // Normalized motor thrust commands before thrust model factor is applied _actuator_motors_sub.copy(&actuator_motors); // Check individual ESC reports - for (int esc_status_idx = 0; esc_status_idx < limited_esc_count; esc_status_idx++) { - - const esc_report_s &cur_esc_report = esc_status.esc[esc_status_idx]; - + for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) { // Map the esc status index to the actuator function index - const unsigned i_esc = cur_esc_report.actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1; + const uint8_t actuator_function_index = + esc_status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1; - if (i_esc >= actuator_motors_s::NUM_CONTROLS) { + if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) { + continue; // Invalid mapping + } + + const bool timeout = now > esc_status.esc[i].timestamp + 300_ms; + 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; } - // Check if ESC telemetry was available and valid at some point. This is a prerequisite for the failure detection. - if (!(_motor_failure_esc_valid_current_mask & (1 << i_esc)) && cur_esc_report.esc_current > 0.0f) { - _motor_failure_esc_valid_current_mask |= (1 << i_esc); - } + _motor_failure_mask &= ~(1u << actuator_function_index); // Reset bit in mask to be able to accumulate failures + _motor_failure_mask |= (static_cast(timeout) << actuator_function_index); // Telemetry timeout - // Check for telemetry timeout - const bool esc_timed_out = now > cur_esc_report.timestamp + 300_ms; - const bool esc_was_valid = _motor_failure_esc_valid_current_mask & (1 << i_esc); - const bool esc_timeout_currently_flagged = _motor_failure_esc_timed_out_mask & (1 << i_esc); + // Current too low + if (_esc_has_reported_current[i]) { + float thrust = 0.f; - if (esc_was_valid && esc_timed_out && !esc_timeout_currently_flagged) { - // Set flag - _motor_failure_esc_timed_out_mask |= (1 << i_esc); - - } else if (!esc_timed_out && esc_timeout_currently_flagged) { - // Reset flag - _motor_failure_esc_timed_out_mask &= ~(1 << i_esc); - } - - // Check if ESC current is too low - if (cur_esc_report.esc_current > FLT_EPSILON) { - _motor_failure_esc_has_current[i_esc] = true; - } - - if (_motor_failure_esc_has_current[i_esc]) { - float esc_throttle = 0.f; - - if (PX4_ISFINITE(actuator_motors.control[i_esc])) { - esc_throttle = fabsf(actuator_motors.control[i_esc]); + if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) { + // NAN means motor is turned off -> 0 thrust + thrust = fabsf(actuator_motors.control[actuator_function_index]); } - const bool throttle_above_threshold = esc_throttle > _param_fd_act_mot_thr.get(); - const bool current_too_low = cur_esc_report.esc_current < esc_throttle * - _param_fd_act_mot_c2t.get(); + bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get(); + bool current_too_low = current < thrust * _param_fd_act_mot_c2t.get(); - if (throttle_above_threshold && current_too_low && !esc_timed_out) { - if (_motor_failure_undercurrent_start_time[i_esc] == 0) { - _motor_failure_undercurrent_start_time[i_esc] = now; - } + _esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms); - } else { - if (_motor_failure_undercurrent_start_time[i_esc] != 0) { - _motor_failure_undercurrent_start_time[i_esc] = 0; - } + 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 (_motor_failure_undercurrent_start_time[i_esc] != 0 - && now > (_motor_failure_undercurrent_start_time[i_esc] + (_param_fd_act_mot_tout.get() * 1_ms)) - && (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) { - // Set flag - _motor_failure_esc_under_current_mask |= (1 << i_esc); - - } // else: this flag is never cleared, as the motor is stopped, so throttle < threshold + _motor_failure_mask |= (static_cast(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index); } } - bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0); - - if (critical_esc_failure && !(_failure_detector_status.flags.motor)) { - // Add motor failure flag to bitfield - _failure_detector_status.flags.motor = true; - - } else if (!critical_esc_failure && _failure_detector_status.flags.motor) { - // Reset motor failure flag - _failure_detector_status.flags.motor = false; - } + _failure_detector_status.flags.motor = (_motor_failure_mask != 0u); } else { // Disarmed - // reset ESC bitfield - for (int i_esc = 0; i_esc < actuator_motors_s::NUM_CONTROLS; i_esc++) { - _motor_failure_undercurrent_start_time[i_esc] = 0; + for (uint8_t i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { + _esc_undercurrent_hysteresis[i].set_state_and_update(false, now); } - _motor_failure_esc_under_current_mask = 0; _failure_detector_status.flags.motor = false; } } diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 78b4861a7a..26ae114abb 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -111,11 +111,9 @@ private: hrt_abstime _imu_status_timestamp_prev{0}; // Motor failure check - uint8_t _motor_failure_esc_valid_current_mask{}; // ESC 1-8, true if ESC telemetry was valid at some point - uint8_t _motor_failure_esc_timed_out_mask{}; // ESC telemetry no longer available -> failure - uint8_t _motor_failure_esc_under_current_mask{}; // ESC drawing too little current -> failure - bool _motor_failure_esc_has_current[actuator_motors_s::NUM_CONTROLS] {false}; // true if some ESC had non-zero current (some don't support it) - hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {}; + bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; // true if some ESC had non-zero current (some don't support it) + systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX]; + uint16_t _motor_failure_mask = 0; // actuator function indexed uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance