Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2025-12-07 01:37:07 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

FailureDetector: use robust timeout checks for motor failure detection (#25757)

Browse Source
This commit is contained in:
Matthias Grob
2025-10-14 20:46:29 +02:00
committed by GitHub
parent 376f52f51d
commit babe094d06
1 changed files with 4 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@@ -255,10 +255,9 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
// First wait for some ESC telemetry that has the required fields. Before that happens, don't check this ESC
// Then check
const hrt_abstime time_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{};
@@ -282,9 +281,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
}
// Check for telemetry timeout
const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp;
const bool esc_timed_out = telemetry_age > 300_ms;
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);
@@ -315,7 +312,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
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] = time_now;
_motor_failure_undercurrent_start_time[i_esc] = now;
}
} else {
@@ -325,7 +322,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
}
if (_motor_failure_undercurrent_start_time[i_esc] != 0
&& (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
&& now > (_motor_failure_undercurrent_start_time[i_esc] + (_param_fd_motor_time_thres.get() * 1_ms))
&& (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
// Set flag
_motor_failure_esc_under_current_mask |= (1 << i_esc);