Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

FailureDetector: rework motor status check

Browse Source
This commit is contained in:
Matthias Grob
2025-12-16 14:24:55 +01:00
parent b4b1b44c6a
commit f8329ff80d
2 changed files with 43 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/FailureDetector.cpp
+40 -78
View File
@@ -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<uint16_t>(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<uint16_t>(_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;
}
}
src/modules/commander/failure_detector/FailureDetector.hpp
+3 -5
View File
@@ -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