fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-22 01:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FailureDetector: check if ESCs have current

Browse Source 
And increase the timeout to 300ms, as some ESCs only update with 10Hz.
This commit is contained in:
Beat Küng
2022-06-03 09:53:56 +02:00
parent 8bf18e31be
commit d1142008f6
2 changed files with 29 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/FailureDetector.cpp
+28 -23
View File
@@ -384,7 +384,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
// Check for telemetry timeout // Check for telemetry timeout
const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp; const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp;
const bool esc_timed_out = telemetry_age > 100_ms; // TODO: magic number const bool esc_timed_out = telemetry_age > 300_ms;
const bool esc_was_valid = _motor_failure_esc_valid_current_mask & (1 << i_esc); 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); const bool esc_timeout_currently_flagged = _motor_failure_esc_timed_out_mask & (1 << i_esc);
@@ -399,35 +399,40 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
} }
// Check if ESC current is too low // Check if ESC current is too low
float esc_throttle = 0.f; if (cur_esc_report.esc_current > FLT_EPSILON) {
_motor_failure_escs_have_current = true;
if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
esc_throttle = fabsf(actuator_motors.control[i_esc]);
} }
const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get(); if (_motor_failure_escs_have_current) {
const bool current_too_low = cur_esc_report.esc_current < esc_throttle * float esc_throttle = 0.f;
_param_fd_motor_current2throttle_thres.get();
if (throttle_above_threshold && current_too_low && !esc_timed_out) { if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
if (_motor_failure_undercurrent_start_time[i_esc] == 0) { esc_throttle = fabsf(actuator_motors.control[i_esc]);
_motor_failure_undercurrent_start_time[i_esc] = time_now;
} }
} else { const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
if (_motor_failure_undercurrent_start_time[i_esc] != 0) { const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
_motor_failure_undercurrent_start_time[i_esc] = 0; _param_fd_motor_current2throttle_thres.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] = time_now;
}
} else {
if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
_motor_failure_undercurrent_start_time[i_esc] = 0;
}
} }
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
&& (_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
} }
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
&& (_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
} }
bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0); bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
src/modules/commander/failure_detector/FailureDetector.hpp
+1
View File
@@ -129,6 +129,7 @@ private:
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_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_timed_out_mask{}; // ESC telemetry no longer available -> failure
uint8_t _motor_failure_esc_under_current_mask{}; // ESC drawing too little current -> failure uint8_t _motor_failure_esc_under_current_mask{}; // ESC drawing too little current -> failure
bool _motor_failure_escs_have_current{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] {}; hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};