work on: feed back checks to commander

src/modules/commander/Commander.hpp

@@ -224,7 +224,7 @@ private:
 
 	Failsafe		_failsafe_instance{this};
 	FailsafeBase		&_failsafe{_failsafe_instance};
-	FailureDetector		_failure_detector{this};
+	FailureDetector		_failure_detector{this, _health_and_arming_checks};
 	HealthAndArmingChecks	_health_and_arming_checks{this, _vehicle_status};
 	MulticopterThrowLaunch  _multicopter_throw_launch{this};
 	Safety			_safety{};

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -109,6 +109,9 @@ public:
 
 	const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
 
+	uint16_t getMotorFailureMask() const {return _esc_checks.getMotorFailureMask(); }
+	bool getEscArmStatus() const { return _esc_checks.getEscArmStatus(); }
+
 #ifndef CONSTRAINED_FLASH
 	ExternalChecks &externalChecks() { return _external_checks; }
 #endif

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

@@ -73,17 +73,30 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
 {
 	const hrt_abstime now = hrt_absolute_time();
 	const hrt_abstime esc_telemetry_timeout =
-		700_ms; // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
+		2_s; // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
 
 	esc_status_s esc_status;
 
-	if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + esc_telemetry_timeout)) {
 
-		checkEscStatus(context, reporter, esc_status);
+	// Run motor status checks even when no new ESC data arrives (to detect timeouts)
+	if (_esc_status_sub.copy(&esc_status)) {
+		if (esc_status.esc_count <= 0) {
+			return;
+		}
+		uint16_t mask = 0;
+
+		mask |= checkEscOnline(context, reporter, esc_status);
+		mask |= checkEscStatus(context, reporter, esc_status);
+		updateEscsStatus(context, reporter, esc_status);
+
+		if (!_param_fd_act_en.get()) {
+			mask |= checkMotorStatus(context, reporter, esc_status);
+		}
+		_motor_failure_mask = mask;
 		reporter.setIsPresent(health_component_t::motors_escs);
 
 	} else if (_param_com_arm_chk_escs.get()
-		   && now - _start_time > 5_s) { // Wait a bit after startup to allow esc's to init
+		   && now - _start_time > esc_telemetry_timeout) { // Wait a bit after startup to allow esc's to init
 
 		/* EVENT
 		 * @description
@@ -100,42 +113,217 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
 	}
 }
 
-void EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
+uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status)
 {
-	const NavModes required_modes = _param_com_arm_chk_escs.get() ? NavModes::All : NavModes::None;
-
-	if (esc_status.esc_count <= 0) {
-		return;
+	// Check if one or more the ESCs are offline
+	if (!_param_com_arm_chk_escs.get()){
+		return 0;
 	}
-
+	uint16_t mask = 0;
 	char esc_fail_msg[50];
 	esc_fail_msg[0] = '\0';
 
-	int online_bitmask = (1 << esc_status.esc_count) - 1;
+	for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
 
-	// Check if one or more the ESCs are offline
-	if (online_bitmask != esc_status.esc_online_flags) {
+		// check if mapped
+		if (!math::isInRange(esc_status.esc[index].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
+							    uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) {
+			continue; // Skip unmapped ESC status entries
+		}
+		const uint8_t actuator_function_index =
+		esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
 
-		for (int index = 0; index < esc_status.esc_count; index++) {
-			if ((esc_status.esc_online_flags & (1 << index)) == 0) {
-				uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1;
+		const bool timeout = hrt_absolute_time() > esc_status.esc[index].timestamp + 300_ms;
+		const bool is_offline = (esc_status.esc_online_flags & (1 << index)) == 0;
+
+
+
+		// Set failure bits for this motor
+		if (timeout || is_offline) {
+			mask |= (1u << actuator_function_index);
+
+			uint8_t motor_index = actuator_function_index + 1;
+			/* EVENT
+				* @description
+				* <profile name="dev">
+				* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
+				* </profile>
+				*/
+			reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs, events::ID("check_escs_offline"),
+							events::Log::Critical, "ESC {1} offline", motor_index);
+			snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", motor_index);
+			esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
+		}
+	}
+
+	if (reporter.mavlink_log_pub() && esc_fail_msg[0] != '\0') {
+		mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
+
+	}
+	return mask;
+
+}
+
+uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
+{
+	if (!_param_com_arm_chk_escs.get()){
+		return 0;
+	}
+
+	uint16_t mask = 0;
+
+	for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
+
+		if (!math::isInRange(esc_status.esc[index].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
+							    uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) {
+			continue; // Skip unmapped ESC status entries
+		}
+
+		const uint8_t act_function_index =
+			esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
+
+		if (esc_status.esc[index].failures == 0) {
+			continue;
+		}
+		else
+		{
+			mask |= (1u << act_function_index); // Set bit in mask
+		}
+
+
+		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))) {
+				continue;
+			}
+
+			esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
+
+			const char *user_action = "";
+			events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
+
+			if (context.isArmed()) {
+				if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
+				    || fault_reason_index == esc_fault_reason_t::esc_warn_temp
+				    || fault_reason_index == esc_fault_reason_t::over_rpm) {
+					user_action = "Reduce throttle";
+					action = events::px4::enums::suggested_action_t::reduce_throttle;
+
+				} else {
+					user_action = "Land now!";
+					action = events::px4::enums::suggested_action_t::land;
+				}
+			}
+
+			/* EVENT
+				* @description
+				* {3}
+				*
+				* <profile name="dev">
+				* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
+				* </profile>
+				*/
+			reporter.healthFailure<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>(
+				NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
+				events::Log::Critical, "ESC {1}: {2}", act_function_index + 1, fault_reason_index, action);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", act_function_index + 1,
+						      esc_fault_reason_str(fault_reason_index), user_action);
+			}
+		}
+	}
+	return mask;
+}
+
+uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
+{
+	if (!_param_fd_act_en.get()){
+		return 0;
+	}
+	const hrt_abstime now = hrt_absolute_time();
+	uint16_t mask = 0;
+
+	// Only check while armed
+	if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		actuator_motors_s actuator_motors{};
+		_actuator_motors_sub.copy(&actuator_motors);
+
+		// Check individual ESC reports
+		for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
+			// Map the esc status index to the actuator function index
+			const uint8_t actuator_function_index =
+				esc_status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
+
+			if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) {
+				continue; // Invalid mapping
+			}
+
+			const float current = esc_status.esc[i].esc_current;
+
+			// First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.
+			if (current > FLT_EPSILON) {
+				_esc_has_reported_current[i] = true;
+			}
+
+			if (!_esc_has_reported_current[i]) {
+				continue;
+			}
+
+			// Current limits
+			float thrust = 0.f;
+
+			if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) {
+				// Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust
+				thrust = fabsf(actuator_motors.control[actuator_function_index]);
+			}
+
+			bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get();
+			bool current_too_low = current < (thrust * _param_fd_act_mot_c2t.get()) - _param_fd_act_low_off.get();
+			bool current_too_high = current > (thrust * _param_fd_act_mot_c2t.get()) + _param_fd_act_high_off.get();
+
+			_esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
+			_esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
+
+			if (!_esc_undercurrent_hysteresis[i].get_state()) {
+				// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
+				_esc_undercurrent_hysteresis[i].set_state_and_update(thrust_above_threshold && current_too_low, now);
+			}
+
+			if (!_esc_overcurrent_hysteresis[i].get_state()) {
+				// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
+				_esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high, now);
+			}
+
+			mask |= (static_cast<uint16_t>(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index);
+			mask |= (static_cast<uint16_t>(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index);
+
+			if (_esc_undercurrent_hysteresis[i].get_state()) {
 				/* EVENT
 					* @description
 					* <profile name="dev">
-					* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
+					* This check can be configured via <param>FD_ACT_EN</param> parameter.
 					* </profile>
 					*/
-				reporter.healthFailure<uint8_t>(required_modes, health_component_t::motors_escs, events::ID("check_escs_offline"),
-								events::Log::Critical, "ESC {1} offline", motor_index);
-				snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", motor_index);
-				esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
-			}
-		}
+				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs,
+								events::ID("check_failure_detector_motor_uc"),
+								events::Log::Critical, "Motor {1} undercurrent detected", actuator_function_index + 1);
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
-		}
-	}
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "Motor failure: Motor %d undercurrent detected",
+							     actuator_function_index + 1);
+				}
+			}
+
+			if (_esc_overcurrent_hysteresis[i].get_state()) {
+				/* 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_oc"),
+								events::Log::Critical, "Motor {1} overcurrent detected", actuator_function_index + 1);
 
 		for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) {
 
@@ -188,36 +376,51 @@ 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;
+	} else { // Disarmed
+		for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
+			_esc_undercurrent_hysteresis[i].set_state_and_update(false, now);
+			_esc_overcurrent_hysteresis[i].set_state_and_update(false, now);
+		}
 
-			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);
+	}
+	return mask;
+}
 
-				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);
-							}
-						}
-					}
-				}
+void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
+{
+	if (!_param_com_arm_chk_escs.get()){
+		return;
+	}
+	hrt_abstime now = hrt_absolute_time();
+
+	if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
+		const int all_escs_armed_mask = (1 << limited_esc_count) - 1;
+		const bool is_all_escs_armed = (all_escs_armed_mask == esc_status.esc_armed_flags);
+
+		_esc_arm_hysteresis.set_hysteresis_time_from(false, 300_ms);
+		_esc_arm_hysteresis.set_state_and_update(!is_all_escs_armed, now);
+
+		if (_esc_arm_hysteresis.get_state()) {
+			/* EVENT
+				* @description
+				* <profile name="dev">
+				* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
+				* </profile>
+				*/
+			reporter.healthFailure(NavModes::All, health_component_t::motors_escs,
+					       events::ID("check_escs_not_all_armed"),
+					       events::Log::Critical, "Not all ESCs are armed");
+
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "ESC failure: Not all ESCs are armed. Land now!");
 			}
 		}
-	}
 
+	} else {
+		// reset ESC bitfield
+		_esc_arm_hysteresis.set_state_and_update(false, now);
+	}
 }

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

@@ -35,7 +35,9 @@
 
 #include "../Common.hpp"
 
+#include <lib/hysteresis/hysteresis.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/failure_detector_status.h>
 
@@ -47,15 +49,38 @@ public:
 
 	void checkAndReport(const Context &context, Report &reporter) override;
 
+	uint16_t getMotorFailureMask() const {
+				return _motor_failure_mask; }
+	bool getEscArmStatus() const { return _esc_arm_hysteresis.get_state(); }
+
 private:
-	void checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
+	uint16_t checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status);
+	uint16_t checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
+	uint16_t checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
+	void updateEscsStatus(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)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 
 	const hrt_abstime _start_time{hrt_absolute_time()};
 
+	uint16_t _motor_failure_mask{0};
+	esc_status_s _last_esc_status{};
+	bool _esc_status_received{false};
+	bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {};
+	systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
+	systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
+	systemlib::Hysteresis _esc_arm_hysteresis;
+
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs
-				       )
+					(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs,
+					(ParamBool<px4::params::FD_ACT_EN>) _param_fd_act_en,
+					(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_act_mot_thr,
+					(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_act_mot_c2t,
+					(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_act_mot_tout,
+					(ParamFloat<px4::params::FD_ACT_LOW_OFF>) _param_fd_act_low_off,
+					(ParamFloat<px4::params::FD_ACT_HIGH_OFF>) _param_fd_act_high_off);
 };

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

@@ -99,26 +99,6 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor
 			 vehicle_status_s::FAILURE_EXT);
 
 
-	reporter.failsafeFlags().fd_esc_arming_failure = context.status().failure_detector_status &
-			vehicle_status_s::FAILURE_ARM_ESC;
-
-	if (reporter.failsafeFlags().fd_esc_arming_failure) {
-		/* EVENT
-		 * @description
-		 * One or more ESCs failed to arm.
-		 *
-		 * <profile name="dev">
-		 * This check can be configured via <param>FD_ESCS_EN</param> parameter.
-		 * </profile>
-		 */
-		reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
-				       events::Log::Critical, "ESC failure");
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected");
-		}
-	}
-
 	reporter.failsafeFlags().fd_imbalanced_prop = context.status().failure_detector_status &
 			vehicle_status_s::FAILURE_IMBALANCED_PROP;
 

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -39,11 +39,13 @@
 */
 
 #include "FailureDetector.hpp"
+#include "../HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 
 using namespace time_literals;
 
-FailureDetector::FailureDetector(ModuleParams *parent) :
-	ModuleParams(parent)
+FailureDetector::FailureDetector(ModuleParams *parent, HealthAndArmingChecks &health_and_arming_checks) :
+	ModuleParams(parent),
+	_health_and_arming_checks(health_and_arming_checks)
 {
 }
 
@@ -67,24 +69,6 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		_failure_detector_status.flags.ext = false;
 	}
 
-	// esc_status subscriber is shared between subroutines
-	esc_status_s esc_status;
-
-	if (_esc_status_sub.update(&esc_status)) {
-		_failure_injector.manipulateEscStatus(esc_status);
-		_last_esc_status = esc_status;
-		_esc_status_received = true;
-
-		if (_param_escs_en.get()) {
-			updateEscsStatus(vehicle_status, esc_status);
-		}
-	}
-
-	// Run motor status checks even when no new ESC data arrives (to detect timeouts)
-	if (_esc_status_received) {
-		updateMotorStatus(vehicle_status, _last_esc_status);
-	}
-
 	if (_param_fd_imb_prop_thr.get() > 0) {
 		updateImbalancedPropStatus();
 	}
@@ -99,12 +83,12 @@ void FailureDetector::publishStatus()
 	failure_detector_status.fd_pitch = _failure_detector_status.flags.pitch;
 	failure_detector_status.fd_alt = _failure_detector_status.flags.alt;
 	failure_detector_status.fd_ext = _failure_detector_status.flags.ext;
-	failure_detector_status.fd_arm_escs = _failure_detector_status.flags.arm_escs;
+	failure_detector_status.fd_arm_escs = _health_and_arming_checks.getEscArmStatus();
 	failure_detector_status.fd_battery = _failure_detector_status.flags.battery;
 	failure_detector_status.fd_imbalanced_prop = _failure_detector_status.flags.imbalanced_prop;
-	failure_detector_status.fd_motor = _failure_detector_status.flags.motor;
+	failure_detector_status.fd_motor = (_health_and_arming_checks.getMotorFailureMask() != 0) || (_failure_injector.getMotorStopMask() != 0);
 	failure_detector_status.imbalanced_prop_metric = _imbalanced_prop_lpf.getState();
-	failure_detector_status.motor_failure_mask = _motor_failure_mask;
+	failure_detector_status.motor_failure_mask = _health_and_arming_checks.getMotorFailureMask();
 	failure_detector_status.motor_stop_mask = _failure_injector.getMotorStopMask();
 	failure_detector_status.timestamp = hrt_absolute_time();
 	_failure_detector_status_pub.publish(failure_detector_status);
@@ -175,32 +159,6 @@ void FailureDetector::updateExternalAtsStatus()
 	}
 }
 
-void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
-{
-	hrt_abstime now = hrt_absolute_time();
-
-	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-		const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
-		const int all_escs_armed_mask = (1 << limited_esc_count) - 1;
-		const bool is_all_escs_armed = (all_escs_armed_mask == esc_status.esc_armed_flags);
-
-		bool is_esc_failure = !is_all_escs_armed;
-
-		for (int i = 0; i < limited_esc_count; i++) {
-			is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0);
-		}
-
-		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
-		_esc_failure_hysteresis.set_state_and_update(!is_all_escs_armed, time_now);
-
-		_failure_detector_status.flags.arm_escs = _esc_failure_hysteresis.get_state();
-
-	} else {
-		// reset ESC bitfield
-		_esc_failure_hysteresis.set_state_and_update(false, now);
-		_failure_detector_status.flags.arm_escs = false;
-	}
-}
 
 void FailureDetector::updateImbalancedPropStatus()
 {
@@ -260,115 +218,3 @@ 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
-
-	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!
-	_motor_failure_mask = 0;
-
-
-	// Check telemetry timeout always, current checks only when armed
-	actuator_motors_s actuator_motors{};
-	_actuator_motors_sub.copy(&actuator_motors);
-
-	// Check individual ESC reports
-	for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
-		const bool mapped = math::isInRange((int)esc_status.esc[i].actuator_function, (int)OutputFunction::Motor1,
-						    (int)OutputFunction::MotorMax);
-
-		// Skip if ESC is not actually connected
-		if (!mapped) {
-			continue;
-		}
-
-		// Map the esc status index to the actuator function index
-		const uint8_t actuator_function_index =
-			esc_status.esc[i].actuator_function - (int)OutputFunction::Motor1;
-
-		if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) {
-			continue; // Invalid mapping
-		}
-
-		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) {
-			_motor_failure_mask |= (1u << actuator_function_index);
-		}
-
-		if (is_offline) {
-			_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) {
-			continue;
-		}
-
-		// First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.
-		if (current > FLT_EPSILON) {
-			_esc_has_reported_current[i] = true;
-		}
-
-		// Current limits
-		float thrust = 0.f;
-
-		if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) {
-			// Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust
-			thrust = fabsf(actuator_motors.control[actuator_function_index]);
-		}
-
-		bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get();
-		bool current_too_low = current < (thrust * _param_fd_act_mot_c2t.get()) - _param_fd_act_low_off.get();
-		bool current_too_high = current > (thrust * _param_fd_act_mot_c2t.get()) + _param_fd_act_high_off.get();
-
-		_esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
-		_esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
-
-		if (!_esc_undercurrent_hysteresis[i].get_state()) {
-			// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
-			_esc_undercurrent_hysteresis[i].set_state_and_update(thrust_above_threshold && current_too_low && !timeout, now);
-		}
-
-		if (!_esc_overcurrent_hysteresis[i].get_state()) {
-			// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
-			_esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high && !timeout, now);
-		}
-
-		_motor_failure_mask |= (static_cast<uint16_t>(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index);
-		_motor_failure_mask |= (static_cast<uint16_t>(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index);
-	}
-
-
-	// Clear current-related hysteresis when disarmed
-	if (!is_armed) {
-		for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
-			_esc_undercurrent_hysteresis[i].set_state_and_update(false, now);
-			_esc_overcurrent_hysteresis[i].set_state_and_update(false, now);
-		}
-	}
-
-
-	_failure_detector_status.flags.motor = (_motor_failure_mask != 0u);
-}

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -81,10 +81,12 @@ union failure_detector_status_u {
 
 using uORB::SubscriptionData;
 
+class HealthAndArmingChecks;
+
 class FailureDetector : public ModuleParams
 {
 public:
-	FailureDetector(ModuleParams *parent);
+	FailureDetector(ModuleParams *parent, HealthAndArmingChecks &health_and_arming_checks);
 	~FailureDetector() = default;
 
 	bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
@@ -95,29 +97,21 @@ public:
 private:
 	void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
 	void updateExternalAtsStatus();
-	void updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
-	void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
 	void updateImbalancedPropStatus();
 
 	failure_detector_status_u _failure_detector_status{};
 
+	HealthAndArmingChecks &_health_and_arming_checks;
+
 	systemlib::Hysteresis _roll_failure_hysteresis{false};
 	systemlib::Hysteresis _pitch_failure_hysteresis{false};
 	systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
-	systemlib::Hysteresis _esc_failure_hysteresis{false};
 
 	static constexpr float _imbalanced_prop_lpf_time_constant{5.f};
 	AlphaFilter<float> _imbalanced_prop_lpf{};
 	uint32_t _selected_accel_device_id{0};
 	hrt_abstime _imu_status_timestamp_prev{0};
 
-	// Motor failure check
-	bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; // true if ESC reported non-zero current before (some never report any)
-	systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
-	systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
-	uint16_t _motor_failure_mask = 0; // actuator function indexed
-	esc_status_s _last_esc_status{}; // Store last received ESC status
-	bool _esc_status_received{false}; // Track if we've ever received ESC data
 
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance