failure_detector: add motor/ESC failure detection

src/modules/commander/Commander.cpp

@@ -2734,49 +2734,6 @@ Commander::run()
 
 		avoidance_check();
 
-		// engine failure detection
-		// TODO: move out of commander
-		if (_actuator_controls_sub.updated()) {
-			/* Check engine failure
-			 * only for fixed wing for now
-			 */
-			if (!_status_flags.circuit_breaker_engaged_enginefailure_check &&
-			    _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol && _armed.armed) {
-
-				actuator_controls_s actuator_controls{};
-				_actuator_controls_sub.copy(&actuator_controls);
-
-				const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
-				const float current2throttle = _battery_current / throttle;
-
-				if (((throttle > _param_ef_throttle_thres.get()) && (current2throttle < _param_ef_current2throttle_thres.get()))
-				    || _status.engine_failure) {
-
-					const float elapsed = hrt_elapsed_time(&_timestamp_engine_healthy) / 1e6f;
-
-					/* potential failure, measure time */
-					if ((_timestamp_engine_healthy > 0) && (elapsed > _param_ef_time_thres.get())
-					    && !_status.engine_failure) {
-
-						_status.engine_failure = true;
-						_status_changed = true;
-
-						PX4_ERR("Engine Failure");
-						set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, _status);
-					}
-				}
-
-			} else {
-				/* no failure reset flag */
-				_timestamp_engine_healthy = hrt_absolute_time();
-
-				if (_status.engine_failure) {
-					_status.engine_failure = false;
-					_status_changed = true;
-				}
-			}
-		}
-
 		/* check if we are disarmed and there is a better mode to wait in */
 		if (!_armed.armed) {
 			/* if there is no radio control but GPS lock the user might want to fly using
@@ -2822,6 +2779,8 @@ Commander::run()
 
 		/* Check for failure detector status */
 		if (_failure_detector.update(_status, _vehicle_control_mode)) {
+			const bool motor_failure_changed = ((_status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) > 0) !=
+							   _failure_detector.getStatus().flags.motor;
 			_status.failure_detector_status = _failure_detector.getStatus().value;
 			auto fd_status_flags = _failure_detector.getStatusFlags();
 			_status_changed = true;
@@ -2884,6 +2843,67 @@ Commander::run()
 								 (imbalanced_propeller_action_t)_param_com_imb_prop_act.get());
 				}
 			}
+
+			// One-time actions based on motor failure
+			if (motor_failure_changed) {
+				if (fd_status_flags.motor) {
+					mavlink_log_critical(&_mavlink_log_pub, "Motor failure detected\t");
+					events::send(events::ID("commander_motor_failure"), events::Log::Emergency,
+						     "Motor failure! Land immediately");
+					set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, _status);
+
+				} else {
+					mavlink_log_critical(&_mavlink_log_pub, "Motor recovered\t");
+					events::send(events::ID("commander_motor_recovered"), events::Log::Warning,
+						     "Motor recovered, landing still advised");
+					set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, true, _status);
+				}
+			}
+
+			if (fd_status_flags.motor) {
+				switch ((ActuatorFailureActions)_param_com_actuator_failure_act.get()) {
+				case ActuatorFailureActions::AUTO_LOITER:
+					mavlink_log_critical(&_mavlink_log_pub, "Loitering due to actuator failure\t");
+					events::send(events::ID("commander_act_failure_loiter"), events::Log::Warning,
+						     "Loitering due to actuator failure");
+					main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
+					_status_changed = true;
+					break;
+
+				case ActuatorFailureActions::AUTO_LAND:
+					mavlink_log_critical(&_mavlink_log_pub, "Landing due to actuator failure\t");
+					events::send(events::ID("commander_act_failure_land"), events::Log::Warning,
+						     "Landing due to actuator failure");
+					main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, _internal_state);
+					_status_changed = true;
+					break;
+
+				case ActuatorFailureActions::AUTO_RTL:
+					mavlink_log_critical(&_mavlink_log_pub, "Returning home due to actuator failure\t");
+					events::send(events::ID("commander_act_failure_rtl"), events::Log::Warning,
+						     "Returning home due to actuator failure");
+					main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, _internal_state);
+					_status_changed = true;
+					break;
+
+				case ActuatorFailureActions::TERMINATE:
+					if (!_armed.manual_lockdown) {
+						mavlink_log_critical(&_mavlink_log_pub, "Flight termination due to actuator failure\t");
+						events::send(events::ID("commander_act_failure_term"), events::Log::Warning,
+							     "Flight termination due to actuator failure");
+						_status_changed = true;
+						_armed.manual_lockdown = true;
+						send_parachute_command();
+					}
+
+					break;
+
+				default:
+					// nothing to do here
+					break;
+				}
+
+			}
 		}
 
 		// Check wind speed actions if either high wind warning or high wind RTL is enabled
@@ -3070,7 +3090,9 @@ Commander::run()
 			fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs;
 			fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
 			fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
+			fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
 			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
+			fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
 			_failure_detector_status_pub.publish(fd_status);
 		}
 
@@ -3178,8 +3200,6 @@ Commander::get_circuit_breaker_params()
 			CBRK_USB_CHK_KEY);
 	_status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(),
 			CBRK_AIRSPD_CHK_KEY);
-	_status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled_by_val(_param_cbrk_enginefail.get(),
-			CBRK_ENGINEFAIL_KEY);
 	_status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(),
 			CBRK_FLIGHTTERM_KEY);
 	_status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled_by_val(_param_cbrk_velposerr.get(),
@@ -3420,7 +3440,6 @@ Commander::update_control_mode()
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
@@ -3832,8 +3851,6 @@ void Commander::battery_status_check()
 	hrt_abstime oldest_update = hrt_absolute_time();
 	float worst_battery_time_s{NAN};
 
-	_battery_current = 0.0f;
-
 	for (auto &battery_sub : _battery_status_subs) {
 		int index = battery_sub.get_instance();
 		battery_status_s battery;
@@ -3919,8 +3936,6 @@ void Commander::battery_status_check()
 				worst_battery_time_s = battery.time_remaining_s;
 			}
 
-			// Sum up current from all batteries.
-			_battery_current += battery.current_filtered_a;
 		}
 	}
 
@@ -4386,6 +4401,7 @@ void Commander::esc_status_check()
 					esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
 					events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
 							events::px4::enums::suggested_action_t::none;
+					// TODO: use esc_status.esc[index].actuator_function as index after SYS_CTRL_ALLOC becomes default
 					events::send<uint8_t, events::px4::enums::suggested_action_t>(events::ID("commander_esc_offline"),
 							events::Log::Critical, "ESC{1} offline. {2}", index + 1, action);
 				}

src/modules/commander/Commander.hpp

@@ -64,7 +64,6 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/action_request.h>
-#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
@@ -240,9 +239,7 @@ private:
 		(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
 
 		// Engine failure
-		(ParamFloat<px4::params::COM_EF_THROT>) _param_ef_throttle_thres,
-		(ParamFloat<px4::params::COM_EF_C2T>) _param_ef_current2throttle_thres,
-		(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
+		(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
 
 		(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
 		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
@@ -256,7 +253,6 @@ private:
 		(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
 		(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
 		(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
-		(ParamInt<px4::params::CBRK_ENGINEFAIL>) _param_cbrk_enginefail,
 		(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
 		(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
 		(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
@@ -283,6 +279,14 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};
 
+	enum class ActuatorFailureActions {
+		DISABLED = 0,
+		AUTO_LOITER = 1,
+		AUTO_LAND = 2,
+		AUTO_RTL = 3,
+		TERMINATE = 4,
+	};
+
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 
@@ -341,7 +345,6 @@ private:
 
 	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	hrt_abstime	_battery_failsafe_timestamp{0};
-	float		_battery_current{0.0f};
 	uint8_t		_last_connected_batteries{0};
 	uint32_t	_last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {};
 	uint16_t	_last_battery_fault[battery_status_s::MAX_INSTANCES] {};
@@ -368,7 +371,6 @@ private:
 
 	hrt_abstime	_boot_timestamp{0};
 	hrt_abstime	_last_disarmed_timestamp{0};
-	hrt_abstime	_timestamp_engine_healthy{0}; ///< absolute time when engine was healty
 	hrt_abstime	_overload_start{0};		///< time when CPU overload started
 
 	hrt_abstime _led_armed_state_toggle{0};
@@ -405,7 +407,6 @@ private:
 
 	// Subscriptions
 	uORB::Subscription					_action_request_sub {ORB_ID(action_request)};
-	uORB::Subscription					_actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
 	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription					_estimator_selector_status_sub{ORB_ID(estimator_selector_status)};

src/modules/commander/commander_params.c

@@ -128,49 +128,6 @@ PARAM_DEFINE_INT32(COM_HLDL_LOSS_T, 120);
  */
 PARAM_DEFINE_INT32(COM_HLDL_REG_T, 0);
 
-/**
- * Engine Failure Throttle Threshold
- *
- * Engine failure triggers only above this throttle value
- *
- * @group Commander
- * @unit norm
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.01
- */
-PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
-
-/**
- * Engine Failure Current/Throttle Threshold
- *
- * Engine failure triggers only below this current value
- *
- * @group Commander
- * @min 0.0
- * @max 50.0
- * @unit A/%
- * @decimal 2
- * @increment 1
- */
-PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
-
-/**
- * Engine Failure Time Threshold
- *
- * Engine failure triggers only if the throttle threshold and the
- * current to throttle threshold are violated for this time
- *
- * @group Commander
- * @unit s
- * @min 0.0
- * @max 60.0
- * @decimal 1
- * @increment 1
- */
-PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
-
 /**
  * RC loss time threshold
  *
@@ -893,6 +850,23 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
  */
 PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
 
+/**
+ * Set the actuator failure failsafe mode
+ *
+ * Note: actuator failure needs to be enabled and configured via FD_ACT_*
+ * parameters.
+ *
+ * @min 0
+ * @max 3
+ * @value 0 Disabled
+ * @value 1 Hold mode
+ * @value 2 Land mode
+ * @value 3 Return mode
+ * @value 4 Terminate
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0);
+
 /**
  * Flag to enable obstacle avoidance.
  *

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -65,8 +65,17 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		_status.flags.ext = false;
 	}
 
-	if (_param_escs_en.get()) {
-		updateEscsStatus(vehicle_status);
+	// esc_status subscriber is shared between subroutines
+	esc_status_s esc_status;
+
+	if (_esc_status_sub.update(&esc_status)) {
+		if (_param_escs_en.get()) {
+			updateEscsStatus(vehicle_status, esc_status);
+		}
+
+		if (_param_fd_actuator_en.get()) {
+			updateMotorStatus(vehicle_status, esc_status);
+		}
 	}
 
 	if (_param_fd_imb_prop_thr.get() > 0) {
@@ -80,7 +89,7 @@ void FailureDetector::updateAttitudeStatus()
 {
 	vehicle_attitude_s attitude;
 
-	if (_vehicule_attitude_sub.update(&attitude)) {
+	if (_vehicle_attitude_sub.update(&attitude)) {
 
 		const matrix::Eulerf euler(matrix::Quatf(attitude.q));
 		const float roll(euler.phi());
@@ -127,22 +136,26 @@ void FailureDetector::updateExternalAtsStatus()
 	}
 }
 
-void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
+void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
 {
 	hrt_abstime time_now = hrt_absolute_time();
 
 	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-		esc_status_s esc_status;
+		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);
 
-		if (_esc_status_sub.update(&esc_status)) {
-			int all_escs_armed = (1 << esc_status.esc_count) - 1;
+		bool is_esc_failure = !is_all_escs_armed;
 
-			_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
-			_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
+		for (int i = 0; i < limited_esc_count; i++) {
+			is_esc_failure = is_esc_failure | (esc_status.esc[i].failures > 0);
+		}
 
-			if (_esc_failure_hysteresis.get_state()) {
-				_status.flags.arm_escs = true;
-			}
+		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
+		_esc_failure_hysteresis.set_state_and_update(is_esc_failure, time_now);
+
+		if (_esc_failure_hysteresis.get_state()) {
+			_status.flags.arm_escs = true;
 		}
 
 	} else {
@@ -209,3 +222,111 @@ void FailureDetector::updateImbalancedPropStatus()
 		}
 	}
 }
+
+void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
+{
+	// What need to be checked:
+	//
+	// 1. ESC telemetry disappears completely -> dead ESC or power loss on that ESC
+	// 2. ESC failures like overvoltage, overcurrent etc. But DShot driver for example is not populating the field 'esc_report.failures'
+	// 3. Motor current too low. Compare drawn motor current to expected value from a parameter
+	// -- ESC voltage does not really make sense and is highly dependent on the setup
+
+	// First wait for some ESC telemetry that has the required fields. Before that happens, don't check this ESC
+	// Then check
+
+	const hrt_abstime time_now = hrt_absolute_time();
+
+	// Only check while armed
+	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);
+
+		actuator_motors_s actuator_motors{};
+		_actuator_motors_sub.copy(&actuator_motors);
+
+		// Check individual ESC reports
+		for (int esc_status_idx = 0; esc_status_idx < limited_esc_count; esc_status_idx++) {
+
+			const esc_report_s &cur_esc_report = esc_status.esc[esc_status_idx];
+
+			// Map the esc status index to the actuator function index
+			const unsigned i_esc = cur_esc_report.actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
+
+			if (i_esc >= actuator_motors_s::NUM_CONTROLS) {
+				continue;
+			}
+
+			// Check if ESC telemetry was available and valid at some point. This is a prerequisite for the failure detection.
+			if (!(_motor_failure_esc_valid_current_mask & (1 << i_esc)) && cur_esc_report.esc_current > 0.0f) {
+				_motor_failure_esc_valid_current_mask |= (1 << i_esc);
+			}
+
+			// 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_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);
+
+			if (esc_was_valid && esc_timed_out && !esc_timeout_currently_flagged) {
+				// Set flag
+				_motor_failure_esc_timed_out_mask |= (1 << i_esc);
+
+			} else if (!esc_timed_out && esc_timeout_currently_flagged) {
+				// Reset flag
+				_motor_failure_esc_timed_out_mask &= ~(1 << i_esc);
+			}
+
+			// 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]);
+			}
+
+			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
+
+		}
+
+		bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
+
+		if (critical_esc_failure && !(_status.flags.motor)) {
+			// Add motor failure flag to bitfield
+			_status.flags.motor = true;
+
+		} else if (!critical_esc_failure && _status.flags.motor) {
+			// Reset motor failure flag
+			_status.flags.motor = false;
+		}
+
+	} else { // Disarmed
+		// reset ESC bitfield
+		for (int i_esc = 0; i_esc < actuator_motors_s::NUM_CONTROLS; i_esc++) {
+			_motor_failure_undercurrent_start_time[i_esc] = 0;
+		}
+
+		_motor_failure_esc_under_current_mask = 0;
+		_status.flags.motor = false;
+	}
+}

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -51,6 +51,7 @@
 
 // subscriptions
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -69,6 +70,7 @@ union failure_detector_status_u {
 		uint16_t arm_escs : 1;
 		uint16_t battery : 1;
 		uint16_t imbalanced_prop : 1;
+		uint16_t motor : 1;
 	} flags;
 	uint16_t value {0};
 };
@@ -79,16 +81,19 @@ class FailureDetector : public ModuleParams
 {
 public:
 	FailureDetector(ModuleParams *parent);
+	~FailureDetector() = default;
 
 	bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
 	const failure_detector_status_u &getStatus() const { return _status; }
 	const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; }
 	float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
+	uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; }
 
 private:
 	void updateAttitudeStatus();
 	void updateExternalAtsStatus();
-	void updateEscsStatus(const vehicle_status_s &vehicle_status);
+	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 _status{};
@@ -103,11 +108,18 @@ private:
 	uint32_t _selected_accel_device_id{0};
 	hrt_abstime _imu_status_timestamp_prev{0};
 
-	uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
+	// Motor failure check
+	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
+	hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};
+
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance
 	uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@@ -117,6 +129,12 @@ private:
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
 		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
 		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
-		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
+		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
+
+		// Actuator failure
+		(ParamBool<px4::params::FD_ACT_EN>) _param_fd_actuator_en,
+		(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_motor_throttle_thres,
+		(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_motor_current2throttle_thres,
+		(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_motor_time_thres
 	)
 };

src/modules/commander/failure_detector/failure_detector_params.c

@@ -156,3 +156,59 @@ PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
  * @group Failure Detector
  */
 PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
+
+/**
+ * Enable Actuator Failure check
+ *
+ * If enabled, failure detector will verify that for motors, a minimum amount of ESC current per throttle
+ * level is being consumed.
+ * Otherwise this indicates an motor failure.
+ *
+ * @boolean
+ * @reboot_required true
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_ACT_EN, 1);
+
+/**
+ * Motor Failure Throttle Threshold
+ *
+ * Motor failure triggers only above this throttle value.
+ *
+ * @group Failure Detector
+ * @unit norm
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ */
+PARAM_DEFINE_FLOAT(FD_ACT_MOT_THR, 0.2f);
+
+/**
+ * Motor Failure Current/Throttle Threshold
+ *
+ * Motor failure triggers only below this current value
+ *
+ * @group Failure Detector
+ * @min 0.0
+ * @max 50.0
+ * @unit A/%
+ * @decimal 2
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 2.0f);
+
+/**
+ * Motor Failure Time Threshold
+ *
+ * Motor failure triggers only if the throttle threshold and the
+ * current to throttle threshold are violated for this time.
+ *
+ * @group Failure Detector
+ * @unit ms
+ * @min 10
+ * @max 10000
+ * @increment 100
+ */
+PARAM_DEFINE_INT32(FD_ACT_MOT_TOUT, 100);

src/modules/commander/state_machine_helper.cpp

@@ -383,9 +383,6 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
-		} else if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
 		} else if (status_flags.vtol_transition_failure) {
 
 			set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
@@ -434,10 +431,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 	case commander_state_s::MAIN_STATE_AUTO_LOITER:
 
 		/* go into failsafe on a engine failure */
-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else if (status_flags.vtol_transition_failure) {
@@ -475,10 +469,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 		/* require global position and home, also go into failsafe on an engine failure */
 
-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 		} else {
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@@ -488,12 +479,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
 
-		/* require global position and home */
+		// require global position and home
 
-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else {
@@ -503,15 +491,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		break;
 
 	case commander_state_s::MAIN_STATE_ORBIT:
-		if (status.engine_failure) {
-			// failsafe: on engine failure
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-			// Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit
-			// is not possible and therefore the internal_state needs to be adjusted.
-			internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// failsafe: necessary position estimate lost; switching is done in check_invalid_pos_nav_state
 
 			// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
@@ -548,12 +528,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
 	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:
 
-		/* require local position */
+		// require local position
 
-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else if (status_flags.vtol_transition_failure) {
@@ -590,12 +567,10 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 	case commander_state_s::MAIN_STATE_AUTO_LAND:
 
-		/* require local position */
+		// require local position
 
-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
 
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else {
@@ -606,12 +581,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 	case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
 
-		/* must be rotary wing plus same requirements as normal landing */
+		// must be rotary wing plus same requirements as normal landing
 
-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else {