uavcan: unify timeout handling

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

@@ -146,7 +146,8 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
 
 						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::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;
 

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -72,14 +72,17 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 
 	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);
 		}
+	}
 
-		if (_param_fd_act_en.get()) {
+	// Run motor status checks even when no new ESC data arrives (to detect timeouts)
-			updateMotorStatus(vehicle_status, esc_status);
+	if (_param_fd_act_en.get() && _esc_status_received) {
-		}
+		updateMotorStatus(vehicle_status, _last_esc_status);
 	}
 
 	if (_param_fd_imb_prop_thr.get() > 0) {
@@ -188,11 +191,9 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
 		}
 
 		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
-		_esc_failure_hysteresis.set_state_and_update(is_esc_failure, now);
+		_esc_failure_hysteresis.set_state_and_update(!is_all_escs_armed, now);
 
-		if (_esc_failure_hysteresis.get_state()) {
+		_failure_detector_status.flags.arm_escs = _esc_failure_hysteresis.get_state();
-			_failure_detector_status.flags.arm_escs = true;
-		}
 
 	} else {
 		// reset ESC bitfield
@@ -266,74 +267,101 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 	// 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;
 
-	// Only check while armed
+	// Clear the failure mask at the start --> Can recover when issue is resolved!!
-	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+	_motor_failure_mask = 0;
-		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) {
+	// Check telemetry timeout always, current checks only when armed
-				continue; // Invalid mapping
+	actuator_motors_s actuator_motors{};
-			}
+	_actuator_motors_sub.copy(&actuator_motors);
 
-			const bool timeout = now > esc_status.esc[i].timestamp + 300_ms;
+	// Check individual ESC reports
-			const float current = esc_status.esc[i].esc_current;
+	for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
+		const bool mapped = math::isInRange(esc_status.esc[i].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
+						    uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1));
 
-			// First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.
+		// Skip if ESC is not actually connected
-			if (current > FLT_EPSILON) {
+		if (!mapped) {
-				_esc_has_reported_current[i] = true;
+			continue;
-			}
-
-			if (!_esc_has_reported_current[i]) {
-				continue;
-			}
-
-			_motor_failure_mask &= ~(1u << actuator_function_index); // Reset bit in mask to accumulate failures again
-			_motor_failure_mask |= (static_cast<uint16_t>(timeout) << actuator_function_index); // Telemetry timeout
-
-			// 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);
 		}
 
-		_failure_detector_status.flags.motor = (_motor_failure_mask != 0u);
+		// 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;
 
-	} else { // Disarmed
+
+		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;
+
+		// 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);
+		}
+
+
+		// 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;
+		}
+
+		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 because a reaction could be to stop the motor and that would be conidered healty 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 because a reaction could be to stop the motor and that would be conidered healty 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 = false;
 	}
+
+
+	_failure_detector_status.flags.motor = (_motor_failure_mask != 0u);
 }

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -115,6 +115,8 @@ private:
 	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