Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 9 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
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_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
float esc_throttle = 0.f;
if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
esc_throttle = fabsf(actuator_motors.control[i_esc]);
if (cur_esc_report.esc_current > FLT_EPSILON) {
_motor_failure_escs_have_current = true;
}
const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
_param_fd_motor_current2throttle_thres.get();
if (_motor_failure_escs_have_current) {
float esc_throttle = 0.f;
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;
if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
esc_throttle = fabsf(actuator_motors.control[i_esc]);
}
} else {
if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
_motor_failure_undercurrent_start_time[i_esc] = 0;
const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
_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);
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_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_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] {};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};