fd: reorganise motor & esc failures

src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp

@@ -134,7 +134,11 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
 
 		for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) {
 
-			if (esc_status.esc[index].failures != 0) {
+			const bool esc_failure = (esc_status.esc[index].failures != 0);
+			reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
+			const bool motor_failure = (reporter.failsafeFlags().fd_motor_failure);
+
+			if (esc_failure) {
 
 				for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max); fault_index++) {
 					if (esc_status.esc[index].failures & (1 << fault_index)) {
@@ -178,6 +182,39 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
 					}
 				}
 			}
+
+			if (motor_failure) {
+				reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
+
+				if (reporter.failsafeFlags().fd_motor_failure) {
+					// Get the failure detector status to check which motor(s) failed
+					failure_detector_status_s failure_detector_status{};
+					bool have_motor_mask = _failure_detector_status_sub.copy(&failure_detector_status);
+
+					if (have_motor_mask && failure_detector_status.motor_failure_mask != 0) {
+						for (uint8_t motor_index = 0; motor_index < esc_status_s::CONNECTED_ESC_MAX; motor_index++) {
+							if (failure_detector_status.motor_failure_mask & (1 << motor_index)) {
+								/* EVENT
+								 * @description
+								 * <profile name="dev">
+								 * This check can be configured via <param>FD_ACT_EN</param> parameter.
+								 * </profile>
+								 */
+								reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs,
+												events::ID("check_failure_detector_motor"),
+												events::Log::Critical, "Motor {1} failure detected", motor_index);
+
+								if (reporter.mavlink_log_pub()) {
+									mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor %d failure detected",
+											     motor_index);
+								}
+							}
+						}
+					}
+				}
+			}
 		}
+
+
 	}
 }

src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp

@@ -37,6 +37,7 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/failure_detector_status.h>
 
 class EscChecks : public HealthAndArmingCheckBase
 {
@@ -50,6 +51,7 @@ private:
 	void checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
 
 	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
+	uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
 
 	const hrt_abstime _start_time{hrt_absolute_time()};
 

src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp

@@ -139,21 +139,6 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor
 		}
 	}
 
-	reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
 
-	if (reporter.failsafeFlags().fd_motor_failure) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * This check can be configured via <param>FD_ACT_EN</param> parameter.
-		 * </profile>
-		 */
-		reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_motor"),
-				       events::Log::Critical, "Motor failure detected");
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor failure detected");
-		}
-	}
 
 }

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -262,6 +262,13 @@ void FailureDetector::updateImbalancedPropStatus()
 
 void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
 {
+	// This check can be configured via <param>FD_ACT_EN</param> parameter.
+
+	if (!_param_fd_act_en.get()) {
+		_failure_detector_status.flags.motor = false;
+		return;
+	}
+
 	// 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
@@ -269,7 +276,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 	const hrt_abstime now = hrt_absolute_time();
 	const bool is_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 
-	// Clear the failure mask at the start --> Can recover when issue is resolved!!
+	// Clear the failure mask at the start --> Can recover when issue is resolved!
 	_motor_failure_mask = 0;
 
 
@@ -299,6 +306,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 		const bool timeout = now > esc_status.esc[i].timestamp + 300_ms;
 		const bool is_offline = (esc_status.esc_online_flags & (1 << i)) == 0;
 		const float current = esc_status.esc[i].esc_current;
+		const bool esc_flag = esc_status.esc[i].failures != 0;
 
 		// Set failure bits for this motor
 		if (timeout) {
@@ -309,6 +317,10 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 			_motor_failure_mask |= (1u << actuator_function_index);
 		}
 
+		if (esc_flag) {
+			_motor_failure_mask |= (1u << actuator_function_index);
+		}
+
 
 		// Current checks only when armed
 		if (!is_armed) {