From d1142008f6630d520a686dfaaf63ec4ed81321ee Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 3 Jun 2022 09:53:56 +0200
Subject: [PATCH] FailureDetector: check if ESCs have current

And increase the timeout to 300ms, as some ESCs only update with 10Hz.
---
 .../failure_detector/FailureDetector.cpp      | 49 ++++++++++---------
 .../failure_detector/FailureDetector.hpp      |  1 +
 2 files changed, 28 insertions(+), 22 deletions(-)

diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp
index 118171dc62..19f2996e2b 100644
--- a/src/modules/commander/failure_detector/FailureDetector.cpp
+++ b/src/modules/commander/failure_detector/FailureDetector.cpp
@@ -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 (_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);
+				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: this flag is never cleared, as the motor is stopped, so throttle < threshold
+				} 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
+			}
 		}
 
 		bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp
index b3b7d5afa2..ba66c44e3a 100644
--- a/src/modules/commander/failure_detector/FailureDetector.hpp
+++ b/src/modules/commander/failure_detector/FailureDetector.hpp
@@ -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)};