failure_detector: add motor/ESC failure detection

msg/failure_detector_status.msg

@@ -8,5 +8,7 @@ bool fd_ext
 bool fd_arm_escs
 bool fd_battery
 bool fd_imbalanced_prop
+bool fd_motor
 
 float32 imbalanced_prop_metric      # Metric of the imbalanced propeller check (low-passed)
+uint16 motor_failure_mask           # Bit-mask with motor indices, indicating critical motor failures

msg/vehicle_status.msg

@@ -11,14 +11,15 @@ uint8 ARMING_STATE_IN_AIR_RESTORE = 5
 uint8 ARMING_STATE_MAX = 6
 
 # FailureDetector status
-uint8 FAILURE_NONE = 0
+uint16 FAILURE_NONE = 0
-uint8 FAILURE_ROLL = 1 	        # (1 << 0)
+uint16 FAILURE_ROLL = 1              # (1 << 0)
-uint8 FAILURE_PITCH = 2	        # (1 << 1)
+uint16 FAILURE_PITCH = 2             # (1 << 1)
-uint8 FAILURE_ALT = 4 	        # (1 << 2)
+uint16 FAILURE_ALT = 4               # (1 << 2)
-uint8 FAILURE_EXT = 8 	        # (1 << 3)
+uint16 FAILURE_EXT = 8               # (1 << 3)
-uint8 FAILURE_ARM_ESC = 16      # (1 << 4)
+uint16 FAILURE_ARM_ESC = 16          # (1 << 4)
-uint8 FAILURE_BATTERY = 32          # (1 << 5)
+uint16 FAILURE_BATTERY = 32          # (1 << 5)
-uint8 FAILURE_IMBALANCED_PROP = 64 # (1 << 6)
+uint16 FAILURE_IMBALANCED_PROP = 64  # (1 << 6)
+uint16 FAILURE_MOTOR = 128           # (1 << 7)
 
 # HIL
 uint8 HIL_STATE_OFF = 0
@@ -31,7 +32,7 @@ uint8 NAVIGATION_STATE_POSCTL = 2		# Position control mode
 uint8 NAVIGATION_STATE_AUTO_MISSION = 3		# Auto mission mode
 uint8 NAVIGATION_STATE_AUTO_LOITER = 4		# Auto loiter mode
 uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
-uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 	# Auto land on engine failure
+uint8 NAVIGATION_STATE_UNUSED3 = 8 	        # Free slot
 uint8 NAVIGATION_STATE_UNUSED = 9	        # Free slot
 uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
 uint8 NAVIGATION_STATE_UNUSED1 = 11		# Free slot
@@ -85,11 +86,10 @@ uint8 data_link_lost_counter			# counts unique data link lost events
 
 bool high_latency_data_link_lost 			# Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
 
-bool engine_failure				# Set to true if an engine failure is detected
 bool mission_failure				# Set to true if mission could not continue/finish
 bool geofence_violated
 
-uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
+uint16 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
 
 # see SYS_STATUS mavlink message for the following
 # lower 32 bits are for the base flags, higher 32 bits are or the extended flags

msg/vehicle_status_flags.msg

@@ -28,7 +28,6 @@ bool flight_terminated
 
 bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
-bool circuit_breaker_engaged_enginefailure_check
 bool circuit_breaker_flight_termination_disabled
 bool circuit_breaker_engaged_usb_check
 bool circuit_breaker_engaged_posfailure_check	# set to true when the position valid checks have been disabled

src/drivers/osd/atxxxx/atxxxx.cpp

@@ -408,10 +408,6 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state)
 		flight_mode = "AUTO";
 		break;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		flight_mode = "FAILURE";
-		break;
-
 	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		flight_mode = "ACRO";
 		break;

src/drivers/rc_input/crsf_telemetry.cpp

@@ -162,10 +162,6 @@ bool CRSFTelemetry::send_flight_mode()
 		flight_mode = "Auto";
 		break;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		flight_mode = "Failure";
-		break;
-
 	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		flight_mode = "Acro";
 		break;

src/lib/circuit_breaker/circuit_breaker.h

@@ -56,7 +56,6 @@
 #define CBRK_IO_SAFETY_KEY	22027
 #define CBRK_AIRSPD_CHK_KEY	162128
 #define CBRK_FLIGHTTERM_KEY	121212
-#define CBRK_ENGINEFAIL_KEY	284953
 #define CBRK_USB_CHK_KEY	197848
 #define CBRK_VELPOSERR_KEY	201607
 #define CBRK_VTOLARMING_KEY	159753

src/lib/circuit_breaker/circuit_breaker_params.c

@@ -115,22 +115,6 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
  */
 PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
 
-/**
- * Circuit breaker for engine failure detection
- *
- * Setting this parameter to 284953 will disable the engine failure detection.
- * If the aircraft is in engine failure mode the engine failure flag will be
- * set to healthy
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @reboot_required true
- * @min 0
- * @max 284953
- * @category Developer
- * @group Circuit Breaker
- */
-PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
-
 /**
  * Circuit breaker for disabling buzzer
  *

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,
+		(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
-		(ParamFloat<px4::params::COM_EF_C2T>) _param_ef_current2throttle_thres,
-		(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
 
 		(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()) {
+	// esc_status subscriber is shared between subroutines
-		updateEscsStatus(vehicle_status);
+	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)) {
+		bool is_esc_failure = !is_all_escs_armed;
-			int all_escs_armed = (1 << esc_status.esc_count) - 1;
 
-			_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
+		for (int i = 0; i < limited_esc_count; i++) {
-			_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
+			is_esc_failure = is_esc_failure | (esc_status.esc[i].failures > 0);
+		}
 
-			if (_esc_failure_hysteresis.get_state()) {
+		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
-				_status.flags.arm_escs = true;
+		_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)};
+	// Motor failure check
-	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
+	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) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
-			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)) {
 			// 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) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
-			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)) {
 			// 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) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
-			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)) {
 			// 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) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
-			// 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)) {
 			// 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) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
-			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)) {
 			// 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) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
-			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)) {
 			// 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) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
-			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)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else {

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -581,9 +581,10 @@ void FixedwingAttitudeControl::Run()
 						_wheel_ctrl.reset_integrator();
 					}
 
-					/* throttle passed through if it is finite and if no engine failure was detected */
+					/* throttle passed through if it is finite */
-					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])
+					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])) ?
-							&& !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;
+							_att_sp.thrust_body[0] :
+							0.0f;
 
 					/* scale effort by battery status */
 					if (_param_fw_bat_scale_en.get() &&

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -2682,12 +2682,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 		_reinitialize_tecs = false;
 	}
 
-	if (_vehicle_status.engine_failure) {
-		/* Force the slow downwards spiral */
-		pitch_min_rad = radians(-1.0f);
-		pitch_max_rad = radians(5.0f);
-	}
-
 	/* No underspeed protection in landing mode */
 	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
 

src/modules/mavlink/mavlink_messages.cpp

@@ -260,11 +260,6 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
 		break;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		break;
-
 	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
 		break;

src/modules/mavlink/streams/HEARTBEAT.hpp

@@ -131,9 +131,6 @@ private:
 			    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
 				system_status = MAV_STATE_FLIGHT_TERMINATION;
 
-			} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
-				system_status = MAV_STATE_EMERGENCY;
-
 			} else if (vehicle_status_flags.calibration_enabled) {
 				system_status = MAV_STATE_CALIBRATING;
 			}

src/modules/mavlink/streams/HIGH_LATENCY2.hpp

@@ -437,7 +437,7 @@ private:
 				msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
 			}
 
-			if (status.engine_failure) {
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) {
 				msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
 			}
 

src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp

@@ -141,7 +141,6 @@ private:
 			bool vehicle_in_auto_mode = (vehicle_status.timestamp > 0)
 						    && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
-							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER

src/modules/navigator/CMakeLists.txt

@@ -48,7 +48,6 @@ px4_add_module(
 		precland.cpp
 		mission_feasibility_checker.cpp
 		geofence.cpp
-		enginefailure.cpp
 		follow_target.cpp
 		vtol_takeoff.cpp
 	DEPENDS

src/modules/navigator/enginefailure.cpp

@@ -1,140 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/* @file enginefailure.cpp
-* Helper class for a fixedwing engine failure mode
-*
-* @author Thomas Gubler <thomasgubler@gmail.com>
-*/
-#include <string.h>
-#include <stdlib.h>
-#include <math.h>
-#include <fcntl.h>
-
-#include <systemlib/mavlink_log.h>
-#include <systemlib/err.h>
-#include <lib/geo/geo.h>
-#include <navigator/navigation.h>
-#include <px4_platform_common/events.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/mission.h>
-
-#include "navigator.h"
-#include "enginefailure.h"
-
-EngineFailure::EngineFailure(Navigator *navigator) :
-	MissionBlock(navigator),
-	_ef_state(EF_STATE_NONE)
-{
-}
-
-void
-EngineFailure::on_inactive()
-{
-	_ef_state = EF_STATE_NONE;
-}
-
-void
-EngineFailure::on_activation()
-{
-	_ef_state = EF_STATE_NONE;
-	advance_ef();
-	set_ef_item();
-}
-
-void
-EngineFailure::on_active()
-{
-	if (is_mission_item_reached()) {
-		advance_ef();
-		set_ef_item();
-	}
-}
-
-void
-EngineFailure::set_ef_item()
-{
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	pos_sp_triplet->previous = pos_sp_triplet->current;
-	_navigator->set_can_loiter_at_sp(false);
-
-	switch (_ef_state) {
-	case EF_STATE_LOITERDOWN: {
-			//XXX create mission item at ground (below?) here
-
-			_mission_item.lat = _navigator->get_global_position()->lat;
-			_mission_item.lon = _navigator->get_global_position()->lon;
-			_mission_item.altitude_is_relative = false;
-			//XXX setting altitude to a very low value, evaluate other options
-			_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
-			_mission_item.yaw = NAN;
-			_mission_item.loiter_radius = _navigator->get_loiter_radius();
-			_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-
-			_navigator->set_can_loiter_at_sp(true);
-			break;
-		}
-
-	default:
-		break;
-	}
-
-	reset_mission_item_reached();
-
-	/* convert mission item to current position setpoint and make it valid */
-	mission_apply_limitation(_mission_item);
-	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
-	pos_sp_triplet->next.valid = false;
-
-	_navigator->set_position_setpoint_triplet_updated();
-}
-
-void
-EngineFailure::advance_ef()
-{
-	switch (_ef_state) {
-	case EF_STATE_NONE:
-		mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down\t");
-		events::send(events::ID("enginefailure_loitering"), events::Log::Emergency, "Engine failure. Loitering down");
-		_ef_state = EF_STATE_LOITERDOWN;
-		break;
-
-	default:
-		break;
-	}
-}

src/modules/navigator/enginefailure.h

@@ -1,73 +0,0 @@
-/***************************************************************************
- *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * @file enginefailure.h
- * Helper class for a fixedwing engine failure mode
- *
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-#pragma once
-
-#include "navigator_mode.h"
-#include "mission_block.h"
-
-class Navigator;
-
-class EngineFailure : public MissionBlock
-{
-public:
-	EngineFailure(Navigator *navigator);
-	~EngineFailure() = default;
-
-	void on_inactive() override;
-	void on_activation() override;
-	void on_active() override;
-
-private:
-	enum EFState {
-		EF_STATE_NONE = 0,
-		EF_STATE_LOITERDOWN = 1,
-	} _ef_state{EF_STATE_NONE};
-
-	/**
-	 * Set the DLL item
-	 */
-	void		set_ef_item();
-
-	/**
-	 * Move to next EF item
-	 */
-	void		advance_ef();
-
-};

src/modules/navigator/navigator.h

@@ -41,7 +41,6 @@
 
 #pragma once
 
-#include "enginefailure.h"
 #include "follow_target.h"
 #include "geofence.h"
 #include "land.h"
@@ -86,7 +85,7 @@ using namespace time_literals;
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 9
+#define NAVIGATOR_MODE_ARRAY_SIZE 8
 
 class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
@@ -393,7 +392,6 @@ private:
 	Land		_land;			/**< class for handling land commands */
 	PrecLand	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
-	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode (FW only!) */
 	FollowTarget	_follow_target;
 
 	NavigatorMode *_navigation_mode{nullptr};	/**< abstract pointer to current navigation mode class */

src/modules/navigator/navigator_main.cpp

@@ -78,19 +78,17 @@ Navigator::Navigator() :
 	_land(this),
 	_precland(this),
 	_rtl(this),
-	_engineFailure(this),
 	_follow_target(this)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
-	_navigation_mode_array[3] = &_engineFailure;
+	_navigation_mode_array[3] = &_takeoff;
-	_navigation_mode_array[4] = &_takeoff;
+	_navigation_mode_array[4] = &_land;
-	_navigation_mode_array[5] = &_land;
+	_navigation_mode_array[5] = &_precland;
-	_navigation_mode_array[6] = &_precland;
+	_navigation_mode_array[6] = &_vtol_takeoff;
-	_navigation_mode_array[7] = &_vtol_takeoff;
+	_navigation_mode_array[7] = &_follow_target;
-	_navigation_mode_array[8] = &_follow_target;
 
 	_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
 	_handle_reverse_delay = param_find("VT_B_REV_DEL");
@@ -697,11 +695,6 @@ void Navigator::run()
 			_precland.set_mode(PrecLandMode::Required);
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
-			_pos_sp_triplet_published_invalid_once = false;
-			navigation_mode_new = &_engineFailure;
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = &_follow_target;