Move param handles in rc_update into unified struct

- To follow structure & matthias recommendation

ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol

@@ -7,11 +7,6 @@
 
 . ${R}etc/init.d/rc.vtol_defaults
 
-# TODO: Enable motor failure detection when the
-# VTOL no longer reports 0A for all ESCs in SITL
-param set-default FD_ACT_EN 0
-param set-default FD_ACT_MOT_TOUT 500
-
 # param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2
 

ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post

@@ -1,4 +1,6 @@
 
+mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
+
 mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p
 
 # shellcheck disable=SC2154

boards/nxp/ucans32k146/src/clockconfig.c

@@ -110,7 +110,7 @@ const struct clock_configuration_s g_initial_clkconfig = {
 		{
 			.mode        = SCG_SPLL_MONITOR_DISABLE,         /* SPLLCM */
 			.div1        = SCG_ASYNC_CLOCK_DIV_BY_1,         /* SPLLDIV1 */
-			.div2        = SCG_ASYNC_CLOCK_DIV_BY_2,         /* SPLLDIV2 */
+			.div2        = SCG_ASYNC_CLOCK_DIV_BY_1,         /* SPLLDIV2 */
 			.prediv      = 1,                                /* PREDIV */
 			.mult        = 40,                               /* MULT */
 			.src         = 0,                                /* SOURCE */

boards/nxp/ucans32k146/src/periphclocks.c

@@ -101,7 +101,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SPLL_DIV2,
+		.clksrc  = CLK_SRC_SIRC_DIV2,
 	},
 	{
 		.clkname = LPSPI0_CLK,
@@ -110,7 +110,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SPLL_DIV2,
+		.clksrc  = CLK_SRC_SIRC_DIV2,
 	},
 	{
 		.clkname = LPUART0_CLK,
@@ -119,7 +119,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SPLL_DIV2,
+		.clksrc  = CLK_SRC_SIRC_DIV2,
 	},
 	{
 		.clkname = LPUART1_CLK,
@@ -128,7 +128,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SPLL_DIV2,
+		.clksrc  = CLK_SRC_SIRC_DIV2,
 	},
 	{
 		.clkname    = RTC0_CLK,

boards/px4/fmu-v5x/nuttx-config/nsh/defconfig

@@ -54,16 +54,9 @@ CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
 CONFIG_CDCACM_VENDORSTR="Auterion"
 CONFIG_CLOCK_MONOTONIC=y
-CONFIG_DEBUG_ASSERTIONS=y
-CONFIG_DEBUG_ERROR=y
-CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
-CONFIG_DEBUG_SCHED=y
-CONFIG_DEBUG_SCHED_ERROR=y
-CONFIG_DEBUG_SCHED_WARN=y
 CONFIG_DEBUG_SYMBOLS=y
-CONFIG_DEBUG_WARN=y
 CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_GPIO=y

msg/actuator_motors.msg

@@ -4,8 +4,6 @@ uint64 timestamp_sample	    # the timestamp the data this control response is ba
 
 uint16 reversible_flags     # bitset which motors are configured to be reversible
 
-uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
-
 uint8 NUM_CONTROLS = 12
 float32[12] control # range: [-1, 1], where 1 means maximum positive thrust,
                     # -1 maximum negative (if not supported by the output, <0 maps to NaN),

msg/control_allocator_status.msg

@@ -19,5 +19,3 @@ int8 ACTUATOR_SATURATION_LOWER     = -2 # The actuator is saturated (with a valu
 int8[16] actuator_saturation            # Indicates actuator saturation status.
                                         # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved.
                                         # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value.
-
-uint16 handled_motor_failure_mask        # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector

msg/esc_report.msg

@@ -8,8 +8,6 @@ uint8 esc_address					# Address of current ESC (in most cases 1-8 / must be set
 
 uint8 esc_state					# State of ESC - depend on Vendor
 
-uint8 actuator_function				# actuator output function (one of Motor1...MotorN)
-
 uint16 failures					# Bitmask to indicate the internal ESC faults
 
 uint8 FAILURE_OVER_CURRENT = 0 			# (1 << 0)

msg/failure_detector_status.msg

@@ -8,7 +8,5 @@ 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/rc_channels.msg

@@ -21,14 +21,6 @@ uint8 FUNCTION_KILLSWITCH    = 17
 uint8 FUNCTION_TRANSITION    = 18
 uint8 FUNCTION_GEAR          = 19
 uint8 FUNCTION_ARMSWITCH     = 20
-uint8 FUNCTION_FLTBTN_SLOT_1 = 21
-uint8 FUNCTION_FLTBTN_SLOT_2 = 22
-uint8 FUNCTION_FLTBTN_SLOT_3 = 23
-uint8 FUNCTION_FLTBTN_SLOT_4 = 24
-uint8 FUNCTION_FLTBTN_SLOT_5 = 25
-uint8 FUNCTION_FLTBTN_SLOT_6 = 26
-
-uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6
 
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)

msg/vehicle_status.msg

@@ -11,15 +11,14 @@ uint8 ARMING_STATE_IN_AIR_RESTORE = 5
 uint8 ARMING_STATE_MAX = 6
 
 # FailureDetector status
-uint16 FAILURE_NONE = 0
-uint16 FAILURE_ROLL = 1              # (1 << 0)
-uint16 FAILURE_PITCH = 2             # (1 << 1)
-uint16 FAILURE_ALT = 4               # (1 << 2)
-uint16 FAILURE_EXT = 8               # (1 << 3)
-uint16 FAILURE_ARM_ESC = 16          # (1 << 4)
-uint16 FAILURE_BATTERY = 32          # (1 << 5)
-uint16 FAILURE_IMBALANCED_PROP = 64  # (1 << 6)
-uint16 FAILURE_MOTOR = 128           # (1 << 7)
+uint8 FAILURE_NONE = 0
+uint8 FAILURE_ROLL = 1 	        # (1 << 0)
+uint8 FAILURE_PITCH = 2	        # (1 << 1)
+uint8 FAILURE_ALT = 4 	        # (1 << 2)
+uint8 FAILURE_EXT = 8 	        # (1 << 3)
+uint8 FAILURE_ARM_ESC = 16      # (1 << 4)
+uint8 FAILURE_BATTERY = 32          # (1 << 5)
+uint8 FAILURE_IMBALANCED_PROP = 64 # (1 << 6)
 
 # HIL
 uint8 HIL_STATE_OFF = 0
@@ -32,7 +31,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_UNUSED3 = 8 	        # Free slot
+uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 	# Auto land on engine failure
 uint8 NAVIGATION_STATE_UNUSED = 9	        # Free slot
 uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
 uint8 NAVIGATION_STATE_UNUSED1 = 11		# Free slot
@@ -86,10 +85,11 @@ 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
 
-uint16 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
+uint8 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,6 +28,7 @@ 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/cyphal/CanardHandle.cpp

@@ -52,18 +52,18 @@
 #endif // NuttX
 
 
-O1HeapInstance *cyphal_allocator{nullptr};
+O1HeapInstance *uavcan_allocator{nullptr};
 
-static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(cyphal_allocator, amount); }
-static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(cyphal_allocator, pointer); }
+static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(uavcan_allocator, amount); }
+static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(uavcan_allocator, pointer); }
 
 
 CanardHandle::CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes)
 {
-	_cyphal_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
-	cyphal_allocator = o1heapInit(_cyphal_heap, HeapSize, nullptr, nullptr);
+	_uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
+	uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr);
 
-	if (cyphal_allocator == nullptr) {
+	if (uavcan_allocator == nullptr) {
 		PX4_ERR("o1heapInit failed with size %u", HeapSize);
 	}
 
@@ -89,8 +89,8 @@ CanardHandle::~CanardHandle()
 	delete _can_interface;
 	_can_interface = nullptr;
 
-	delete static_cast<uint8_t *>(_cyphal_heap);
-	_cyphal_heap = nullptr;
+	delete static_cast<uint8_t *>(_uavcan_heap);
+	_uavcan_heap = nullptr;
 
 }
 
@@ -200,11 +200,6 @@ CanardTreeNode *CanardHandle::getRxSubscriptions(CanardTransferKind kind)
 	return _canard_instance.rx_subscriptions[kind];
 }
 
-O1HeapDiagnostics CanardHandle::getO1HeapDiagnostics()
-{
-	return o1heapGetDiagnostics(cyphal_allocator);
-}
-
 int32_t CanardHandle::mtu()
 {
 	return _queue.mtu_bytes;

src/drivers/cyphal/CanardHandle.hpp

@@ -34,7 +34,6 @@
 #pragma once
 
 #include <canard.h>
-#include "o1heap/o1heap.h"
 #include "CanardInterface.hpp"
 
 class CanardHandle
@@ -69,7 +68,6 @@ public:
 	int8_t RxUnsubscribe(const CanardTransferKind transfer_kind,
 			     const CanardPortID       port_id);
 	CanardTreeNode *getRxSubscriptions(CanardTransferKind kind);
-	O1HeapDiagnostics getO1HeapDiagnostics();
 
 	int32_t mtu();
 	CanardNodeID node_id();
@@ -82,6 +80,6 @@ private:
 
 	CanardTxQueue _queue;
 
-	void *_cyphal_heap{nullptr};
+	void *_uavcan_heap{nullptr};
 
 };

src/drivers/cyphal/Cyphal.cpp

@@ -245,12 +245,12 @@ void CyphalNode::print_info()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
 
-	O1HeapDiagnostics heap_diagnostics = _canard_handle.getO1HeapDiagnostics();
+	/*O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
 
 	PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64,
 		 heap_diagnostics.allocated, heap_diagnostics.capacity,
 		 heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
-		 heap_diagnostics.oom_count);
+		 heap_diagnostics.oom_count);*/
 
 	_pub_manager.printInfo();
 

src/drivers/dshot/DShot.cpp

@@ -247,10 +247,7 @@ void DShot::update_telemetry_num_motors()
 
 	if (_mixing_output.useDynamicMixing()) {
 		for (unsigned i = 0; i < _num_outputs; ++i) {
-			if (_mixing_output.isFunctionSet(i)) {
-				_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
-				++motor_count;
-			}
+			motor_count += _mixing_output.isFunctionSet(i);
 		}
 
 	} else {
@@ -284,26 +281,24 @@ void DShot::init_telemetry(const char *device)
 	update_telemetry_num_motors();
 }
 
-void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
+void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data)
 {
 	// fill in new motor data
 	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
 
-	if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) {
-		esc_status.esc_online_flags |= 1 << telemetry_index;
+	if (motor_index < esc_status_s::CONNECTED_ESC_MAX) {
+		esc_status.esc_online_flags |= 1 << motor_index;
 
-		esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
-		esc_status.esc[telemetry_index].timestamp       = data.time;
-		esc_status.esc[telemetry_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) /
-				(_param_mot_pole_count.get() / 2);
-		esc_status.esc[telemetry_index].esc_voltage     = static_cast<float>(data.voltage) * 0.01f;
-		esc_status.esc[telemetry_index].esc_current     = static_cast<float>(data.current) * 0.01f;
-		esc_status.esc[telemetry_index].esc_temperature = static_cast<float>(data.temperature);
+		esc_status.esc[motor_index].timestamp       = data.time;
+		esc_status.esc[motor_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) / (_param_mot_pole_count.get() / 2);
+		esc_status.esc[motor_index].esc_voltage     = static_cast<float>(data.voltage) * 0.01f;
+		esc_status.esc[motor_index].esc_current     = static_cast<float>(data.current) * 0.01f;
+		esc_status.esc[motor_index].esc_temperature = static_cast<float>(data.temperature);
 		// TODO: accumulate consumption and use for battery estimation
 	}
 
 	// publish when motor index wraps (which is robust against motor timeouts)
-	if (telemetry_index <= _telemetry->last_telemetry_index) {
+	if (motor_index <= _telemetry->last_motor_index) {
 		esc_status.timestamp = hrt_absolute_time();
 		esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
 		esc_status.esc_count = _telemetry->handler.numMotors();
@@ -319,7 +314,7 @@ void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTele
 		esc_status.esc_online_flags = 0;
 	}
 
-	_telemetry->last_telemetry_index = telemetry_index;
+	_telemetry->last_motor_index = motor_index;
 }
 
 int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)
@@ -498,6 +493,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 void DShot::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -506,8 +503,6 @@ void DShot::Run()
 		return;
 	}
 
-	SmartLock lock_guard(_lock);
-
 	perf_begin(_cycle_perf);
 
 	_mixing_output.update();

src/drivers/dshot/DShot.h

@@ -127,15 +127,14 @@ private:
 	struct Telemetry {
 		DShotTelemetry handler{};
 		uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
-		int last_telemetry_index{-1};
-		uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
+		int last_motor_index{-1};
 	};
 
 	void enable_dshot_outputs(const bool enabled);
 
 	void init_telemetry(const char *device);
 
-	void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
+	void handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data);
 
 	int request_esc_info();
 

src/drivers/linux_pwm_out/linux_pwm_out.cpp

@@ -126,6 +126,8 @@ bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS
 
 void LinuxPWMOut::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -134,8 +136,6 @@ void LinuxPWMOut::Run()
 		return;
 	}
 
-	SmartLock lock_guard(_lock);
-
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 

src/drivers/osd/atxxxx/atxxxx.cpp

@@ -408,6 +408,10 @@ 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/pca9685_pwm_out/main.cpp

@@ -376,6 +376,8 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned
 
 void PCA9685Wrapper::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -389,8 +391,6 @@ void PCA9685Wrapper::Run()
 		return;
 	}
 
-	SmartLock lock_guard(_lock);
-
 	perf_begin(_cycle_perf);
 
 	switch (_state) {

src/drivers/pwm_out/PWMOut.cpp

@@ -439,6 +439,8 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 void PWMOut::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -447,8 +449,6 @@ void PWMOut::Run()
 		return;
 	}
 
-	SmartLock lock_guard(_lock);
-
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 

src/drivers/pwm_out_sim/PWMSim.cpp

@@ -58,6 +58,8 @@ PWMSim::PWMSim(bool hil_mode_enabled) :
 void
 PWMSim::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -66,8 +68,6 @@ PWMSim::Run()
 		return;
 	}
 
-	SmartLock lock_guard(_lock);
-
 	_mixing_output.update();
 
 	// check for parameter updates

src/drivers/px4io/px4io.cpp

@@ -525,6 +525,8 @@ void PX4IO::updateFailsafe()
 
 void PX4IO::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -533,8 +535,6 @@ void PX4IO::Run()
 		return;
 	}
 
-	SmartLock lock_guard(_lock);
-
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 

src/drivers/rc_input/RCInput.cpp

@@ -48,6 +48,12 @@ RCInput::RCInput(const char *device) :
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
 	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
 {
+	// rc input, published to ORB
+	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+
+	// initialize it as RC lost
+	_rc_in.rc_lost = true;
+
 	// initialize raw_rc values and count
 	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
 		_raw_rc_values[i] = UINT16_MAX;
@@ -110,8 +116,6 @@ RCInput::init()
 	px4_arch_unconfiggpio(GPIO_PPM_IN);
 #endif // GPIO_PPM_IN
 
-	rc_io_invert(false);
-
 	return 0;
 }
 
@@ -267,6 +271,8 @@ void RCInput::set_rc_scan_state(RC_SCAN newState)
 
 	_rc_scan_begin = 0;
 	_rc_scan_locked = false;
+
+	_report_lock = true;
 }
 
 void RCInput::rc_io_invert(bool invert)
@@ -411,8 +417,8 @@ void RCInput::Run()
 		bool rc_updated = false;
 
 		// This block scans for a supported serial RC input and locks onto the first one found
-		// Scan for 500 msec, then switch protocol
-		constexpr hrt_abstime rc_scan_max = 500_ms;
+		// Scan for 300 msec, then switch protocol
+		constexpr hrt_abstime rc_scan_max = 300_ms;
 
 		unsigned frame_drops = 0;
 
@@ -428,8 +434,6 @@ void RCInput::Run()
 			_bytes_rx += newBytes;
 		}
 
-		const bool rc_scan_locked = _rc_scan_locked;
-
 		switch (_rc_scan_state) {
 		case RC_SCAN_NONE:
 			// do nothing
@@ -750,18 +754,15 @@ void RCInput::Run()
 			_rc_scan_locked = false;
 		}
 
-		if (!rc_scan_locked && _rc_scan_locked) {
+		if (_report_lock && _rc_scan_locked) {
+			_report_lock = false;
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
-		}
 
-		// set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
-		if (!_armed && rc_updated && _rc_scan_locked
-		    && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
-		    && (_param_rc_input_proto.get() < 0)
-		   ) {
-			// RC_INPUT_PROTO auto => locked selection
-			_param_rc_input_proto.set(_rc_scan_state);
-			_param_rc_input_proto.commit();
+			if (!_armed && (_param_rc_input_proto.get() < 0)) {
+				// RC_INPUT_PROTO auto => locked selection
+				_param_rc_input_proto.set(_rc_scan_state);
+				_param_rc_input_proto.commit();
+			}
 		}
 	}
 }

src/drivers/rc_input/RCInput.hpp

@@ -131,6 +131,7 @@ private:
 
 	bool _initialized{false};
 	bool _rc_scan_locked{false};
+	bool _report_lock{true};
 
 	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
 

src/drivers/rc_input/crsf_telemetry.cpp

@@ -162,6 +162,10 @@ 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/drivers/tap_esc/TAP_ESC.cpp

@@ -290,8 +290,6 @@ bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], u
 
 				if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) {
 					_esc_feedback.esc[feed_back_data.channelID].timestamp = hrt_absolute_time();
-					_esc_feedback.esc[feed_back_data.channelID].actuator_function = (uint8_t)_mixing_output.outputFunction(
-								feed_back_data.channelID);
 					_esc_feedback.esc[feed_back_data.channelID].esc_errorcount = 0;
 					_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
 #if defined(ESC_HAVE_VOLTAGE_SENSOR)
@@ -329,6 +327,8 @@ bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], u
 
 void TAP_ESC::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -337,8 +337,6 @@ void TAP_ESC::Run()
 		return;
 	}
 
-	SmartLock lock_guard(_lock);
-
 	// push backup schedule
 	ScheduleDelayed(20_ms);
 

src/drivers/telemetry/hott/messages.cpp

@@ -48,7 +48,6 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/esc_status.h>
-#include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -115,7 +114,6 @@ publish_gam_message(const uint8_t *buffer)
 	esc.esc_count = 1;
 	esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_PPM;
 
-	esc.esc[0].actuator_function = actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
 	esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
 	esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1 - 20);
 	esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;

src/drivers/uavcan/actuators/esc.hpp

@@ -78,8 +78,6 @@ public:
 
 	static int max_output_value() { return uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); }
 
-	esc_status_s &esc_status() { return _esc_status; }
-
 private:
 	/**
 	 * ESC status message reception will be reported via this callback.

src/drivers/uavcan/sensors/battery.cpp

@@ -87,7 +87,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	battery_status[instance].voltage_filtered_v = msg.voltage;
 	battery_status[instance].current_a = msg.current;
 	battery_status[instance].current_filtered_a = msg.current;
-	battery_status[instance].current_average_a = msg.current;
+	// battery_status[instance].current_average_a = msg.;
 
 	if (battery_aux_support[instance] == false) {
 		sumDischarged(battery_status[instance].timestamp, battery_status[instance].current_a);
@@ -101,11 +101,11 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	battery_status[instance].connected = true;
 	battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
 	// battery_status[instance].priority = msg.;
-	battery_status[instance].capacity = msg.full_charge_capacity_wh;
+	// battery_status[instance].capacity = msg.;
 	battery_status[instance].full_charge_capacity_wh = msg.full_charge_capacity_wh;
 	battery_status[instance].remaining_capacity_wh = msg.remaining_capacity_wh;
 	// battery_status[instance].cycle_count = msg.;
-	battery_status[instance].time_remaining_s = NAN;
+	// battery_status[instance].time_remaining_s = msg.;
 	// battery_status[instance].average_time_to_empty = msg.;
 	battery_status[instance].serial_number = msg.model_instance_id;
 	battery_status[instance].id = msg.getSrcNodeID().get();

src/drivers/uavcan/sensors/battery.hpp

@@ -85,5 +85,5 @@ private:
 	uint8_t _warning;
 	hrt_abstime _last_timestamp;
 	battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {};
-	bool battery_aux_support[battery_status_s::MAX_INSTANCES] {false};
+	bool battery_aux_support[battery_status_s::MAX_INSTANCES] {};
 };

src/drivers/uavcan/uavcan_main.cpp

@@ -1003,10 +1003,6 @@ void UavcanMixingInterfaceESC::mixerChanged()
 	if (_mixing_output.useDynamicMixing()) {
 		for (unsigned i = 0; i < MAX_ACTUATORS; ++i) {
 			rotor_count += _mixing_output.isFunctionSet(i);
-
-			if (i < esc_status_s::CONNECTED_ESC_MAX) {
-				_esc_controller.esc_status().esc[i].actuator_function = (uint8_t)_mixing_output.outputFunction(i);
-			}
 		}
 
 	} else {

src/lib/circuit_breaker/circuit_breaker.h

@@ -56,6 +56,7 @@
 #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,6 +115,22 @@ 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/lib/mixer_module/functions.hpp

@@ -116,8 +116,6 @@ public:
 	static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1,
 		      "Unexpected num motors");
 
-	static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx");
-
 	FunctionMotors(const Context &context);
 	static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); }
 

src/modules/commander/Commander.cpp

@@ -721,7 +721,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 			return TRANSITION_DENIED;
 		}
 
-		if ((_geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL)
+		if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)
 		    && !_status_flags.home_position_valid) {
 			mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t");
 			events::send(events::ID("commander_arm_denied_geofence_rtl"),
@@ -803,12 +803,6 @@ Commander::Commander() :
 {
 	_vehicle_land_detected.landed = true;
 
-	_status.system_id = 1;
-	_status.component_id = 1;
-
-	_status.system_type = 0;
-	_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
-
 	// XXX for now just set sensors as initialized
 	_status_flags.system_sensors_initialized = true;
 
@@ -831,13 +825,6 @@ Commander::Commander() :
 
 	_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
 	_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
-
-	_param_mav_comp_id = param_find("MAV_COMP_ID");
-	_param_mav_sys_id = param_find("MAV_SYS_ID");
-	_param_mav_type = param_find("MAV_TYPE");
-	_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
-
-	updateParameters();
 }
 
 Commander::~Commander()
@@ -2048,84 +2035,15 @@ Commander::updateHomePositionYaw(float yaw)
 	}
 }
 
-void Commander::updateParameters()
-{
-	// update parameters from storage
-	updateParams();
-
-	get_circuit_breaker_params();
-
-	int32_t value_int32 = 0;
-
-	// MAV_SYS_ID => vehicle_status.system_id
-	if ((_param_mav_sys_id != PARAM_INVALID) && (param_get(_param_mav_sys_id, &value_int32) == PX4_OK)) {
-		_status.system_id = value_int32;
-	}
-
-	// MAV_COMP_ID => vehicle_status.component_id
-	if ((_param_mav_comp_id != PARAM_INVALID) && (param_get(_param_mav_comp_id, &value_int32) == PX4_OK)) {
-		_status.component_id = value_int32;
-	}
-
-	// MAV_TYPE -> vehicle_status.system_type
-	if ((_param_mav_type != PARAM_INVALID) && (param_get(_param_mav_type, &value_int32) == PX4_OK)) {
-		_status.system_type = value_int32;
-	}
-
-
-	_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
-
-	_arm_requirements.arm_authorization = _param_arm_auth_required.get();
-	_arm_requirements.esc_check = _param_escs_checks_required.get();
-	_arm_requirements.global_position = !_param_arm_without_gps.get();
-	_arm_requirements.mission = _param_arm_mission_required.get();
-
-	_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
-	_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
-
-	const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) && _vtol_status.vtol_in_rw_mode);
-	const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) && !_vtol_status.vtol_in_rw_mode);
-	const bool is_ground = is_ground_rover(_status);
-
-	/* disable manual override for all systems that rely on electronic stabilization */
-	if (is_rotary) {
-		_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
-
-	} else if (is_fixed) {
-		_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
-
-	} else if (is_ground) {
-		_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
-	}
-
-	_status.is_vtol = is_vtol(_status);
-	_status.is_vtol_tailsitter = is_vtol_tailsitter(_status);
-
-	// CP_DIST: collision preventation enabled if CP_DIST > 0
-	if (is_rotary_wing(_status) || is_vtol(_status)) {
-		if (_param_cp_dist == PARAM_INVALID) {
-			_param_cp_dist = param_find("CP_DIST");
-		}
-
-		float cp_dist = 0;
-
-		if (_param_cp_dist != PARAM_INVALID && (param_get(_param_cp_dist, &cp_dist) == PX4_OK)) {
-			_collision_prevention_enabled = cp_dist > 0.f;
-		}
-	}
-
-	// _mode_switch_mapped = (RC_MAP_FLTMODE > 0)
-	if (_param_rc_map_fltmode != PARAM_INVALID && (param_get(_param_rc_map_fltmode, &value_int32) == PX4_OK)) {
-		_mode_switch_mapped = (value_int32 > 0);
-	}
-
-}
-
 void
 Commander::run()
 {
 	bool sensor_fail_tune_played = false;
 
+	const param_t param_airmode = param_find("MC_AIRMODE");
+	const param_t param_man_arm_gesture = param_find("MAN_ARM_GESTURE");
+	const param_t param_rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
+
 	/* initialize */
 	led_init();
 	buzzer_init();
@@ -2152,6 +2070,29 @@ Commander::run()
 
 #endif // BOARD_HAS_POWER_CONTROL
 
+	get_circuit_breaker_params();
+
+	bool param_init_forced = true;
+
+	/* update vehicle status to find out vehicle type (required for preflight checks) */
+	_status.system_type = _param_mav_type.get();
+
+	if (is_rotary_wing(_status) || is_vtol(_status)) {
+		_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+	} else if (is_fixed_wing(_status)) {
+		_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+
+	} else if (is_ground_rover(_status)) {
+		_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
+
+	} else {
+		_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
+	}
+
+	_status.is_vtol = is_vtol(_status);
+	_status.is_vtol_tailsitter = is_vtol_tailsitter(_status);
+
 	_boot_timestamp = hrt_absolute_time();
 
 	// initially set to failed
@@ -2159,6 +2100,7 @@ Commander::run()
 	_last_gpos_fail_time_us = _boot_timestamp;
 	_last_lvel_fail_time_us = _boot_timestamp;
 
+	_status.system_id = _param_mav_sys_id.get();
 	arm_auth_init(&_mavlink_log_pub, &_status.system_id);
 
 	// run preflight immediately to find all relevant parameters, but don't report
@@ -2172,17 +2114,94 @@ Commander::run()
 		/* update parameters */
 		const bool params_updated = _parameter_update_sub.updated();
 
-		if (params_updated) {
+		if (params_updated || param_init_forced) {
 			// clear update
 			parameter_update_s update;
 			_parameter_update_sub.copy(&update);
 
+			// update parameters from storage
+			updateParams();
+
 			/* update parameters */
 			if (!_armed.armed) {
-				updateParameters();
+				_status.system_type = _param_mav_type.get();
+
+				const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) && _vtol_status.vtol_in_rw_mode);
+				const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) && !_vtol_status.vtol_in_rw_mode);
+				const bool is_ground = is_ground_rover(_status);
+
+				/* disable manual override for all systems that rely on electronic stabilization */
+				if (is_rotary) {
+					_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+				} else if (is_fixed) {
+					_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+
+				} else if (is_ground) {
+					_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
+				}
+
+				/* set vehicle_status.is_vtol flag */
+				_status.is_vtol = is_vtol(_status);
+				_status.is_vtol_tailsitter = is_vtol_tailsitter(_status);
+
+				/* check and update system / component ID */
+				_status.system_id = _param_mav_sys_id.get();
+				_status.component_id = _param_mav_comp_id.get();
+
+				get_circuit_breaker_params();
 
 				_status_changed = true;
 			}
+
+			_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
+
+			_arm_requirements.arm_authorization = _param_arm_auth_required.get();
+			_arm_requirements.esc_check = _param_escs_checks_required.get();
+			_arm_requirements.global_position = !_param_arm_without_gps.get();
+			_arm_requirements.mission = _param_arm_mission_required.get();
+			_arm_requirements.geofence = _param_geofence_action.get() > geofence_result_s::GF_ACTION_NONE;
+
+			_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
+			_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
+
+			// disable arm gesture if an arm switch is configured
+			if (param_man_arm_gesture != PARAM_INVALID && param_rc_map_arm_sw != PARAM_INVALID) {
+				int32_t man_arm_gesture = 0, rc_map_arm_sw = 0;
+				param_get(param_man_arm_gesture, &man_arm_gesture);
+				param_get(param_rc_map_arm_sw, &rc_map_arm_sw);
+
+				if (rc_map_arm_sw > 0 && man_arm_gesture == 1) {
+					man_arm_gesture = 0; // disable arm gesture
+					param_set(param_man_arm_gesture, &man_arm_gesture);
+					mavlink_log_critical(&_mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t")
+					/* EVENT
+					* @description <param>MAN_ARM_GESTURE</param> is now set to disable arm/disarm stick gesture.
+					*/
+					events::send(events::ID("rc_update_arm_stick_gesture_disabled_with_switch"), {events::Log::Info, events::LogInternal::Disabled},
+						     "Arm stick gesture disabled if arm switch in use");
+				}
+			}
+
+			// check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture
+			if (param_airmode != PARAM_INVALID && param_man_arm_gesture != PARAM_INVALID) {
+				int32_t airmode = 0, man_arm_gesture = 0;
+				param_get(param_airmode, &airmode);
+				param_get(param_man_arm_gesture, &man_arm_gesture);
+
+				if (airmode == 2 && man_arm_gesture == 1) {
+					airmode = 1; // change to roll/pitch airmode
+					param_set(param_airmode, &airmode);
+					mavlink_log_critical(&_mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t")
+					/* EVENT
+					 * @description <param>MC_AIRMODE</param> is now set to roll/pitch airmode.
+					 */
+					events::send(events::ID("commander_airmode_requires_no_arm_gesture"), {events::Log::Error, events::LogInternal::Disabled},
+						     "Yaw Airmode requires disabling the stick arm gesture");
+				}
+			}
+
+			param_init_forced = false;
 		}
 
 		/* Update OA parameter */
@@ -2506,16 +2525,16 @@ Commander::run()
 		}
 
 		/* start geofence result check */
-		if (_geofence_result_sub.update(&_geofence_result)) {
-			_arm_requirements.geofence = (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE);
-			_status.geofence_violated = _geofence_result.geofence_violated;
-		}
+		_geofence_result_sub.update(&_geofence_result);
+		_status.geofence_violated = _geofence_result.geofence_violated;
 
 		const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0;
 
 		// Geofence actions
+		const bool geofence_action_enabled = _geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE;
+
 		if (_armed.armed
-		    && (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)
+		    && geofence_action_enabled
 		    && !in_low_battery_failsafe_delay) {
 
 			// check for geofence violation transition
@@ -2662,9 +2681,10 @@ Commander::run()
 					}
 				}
 
+				const bool mode_switch_mapped = (_param_rc_map_fltmode.get() > 0) || (_param_rc_map_mode_sw.get() > 0);
 				const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
 
-				if (!_armed.armed && (is_mavlink || !_mode_switch_mapped) && (_internal_state.main_state_changes == 0)) {
+				if (!_armed.armed && (is_mavlink || !mode_switch_mapped) && (_internal_state.main_state_changes == 0)) {
 					// if there's never been a mode change force position control as initial state
 					_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
 					_internal_state.main_state_changes++;
@@ -2734,6 +2754,49 @@ 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
@@ -2779,8 +2842,6 @@ 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;
@@ -2843,67 +2904,6 @@ 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
@@ -3090,9 +3090,7 @@ 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);
 		}
 
@@ -3172,7 +3170,7 @@ Commander::run()
 
 		_was_armed = _armed.armed;
 
-		arm_auth_update(now, params_updated);
+		arm_auth_update(now, params_updated || param_init_forced);
 
 		px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);
 
@@ -3200,6 +3198,8 @@ 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(),
@@ -3440,6 +3440,7 @@ 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:
@@ -3821,17 +3822,18 @@ void Commander::avoidance_check()
 		}
 	}
 
+	const bool cp_enabled =  _param_cp_dist.get() > 0.f;
+
 	const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms;
 	const bool cp_healthy = _status_flags.avoidance_system_valid || distance_sensor_valid;
 
-	const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || _collision_prevention_enabled;
+	const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || cp_enabled;
 
 	const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled;
 	const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled
 				   && _vehicle_control_mode.flag_control_position_enabled);
 
-	const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode
-					&& _collision_prevention_enabled));
+	const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_enabled));
 	const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy));
 
 	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled,
@@ -3851,6 +3853,8 @@ 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;
@@ -3936,6 +3940,8 @@ 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;
 		}
 	}
 
@@ -4401,7 +4407,6 @@ 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,6 +64,7 @@
 #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>
@@ -183,8 +184,6 @@ private:
 
 	void checkWindSpeedThresholds();
 
-	void updateParameters();
-
 	DEFINE_PARAMETERS(
 
 		(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
@@ -223,6 +222,9 @@ private:
 
 		(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn,
 
+		(ParamInt<px4::params::RC_MAP_FLTMODE>) _param_rc_map_fltmode,
+		(ParamInt<px4::params::RC_MAP_MODE_SW>) _param_rc_map_mode_sw,
+
 		// Quadchute
 		(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
 
@@ -239,7 +241,9 @@ private:
 		(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
 
 		// Engine failure
-		(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
+		(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,
 
 		(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
 		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
@@ -253,21 +257,27 @@ 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,
 
+		// Geofence
+		(ParamInt<px4::params::GF_ACTION>) _param_geofence_action,
+
+		// Mavlink
+		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
+		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
+		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
+
+		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist,
+
+		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
+		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
 		(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
 		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max
 	)
 
-	// optional parameters
-	param_t _param_cp_dist{PARAM_INVALID};
-	param_t _param_mav_comp_id{PARAM_INVALID};
-	param_t _param_mav_sys_id{PARAM_INVALID};
-	param_t _param_mav_type{PARAM_INVALID};
-	param_t _param_rc_map_fltmode{PARAM_INVALID};
-
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@@ -279,14 +289,6 @@ 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};
 
@@ -318,8 +320,6 @@ private:
 	bool		_geofence_warning_action_on{false};
 	bool		_geofence_violated_prev{false};
 
-	bool            _collision_prevention_enabled{false};
-
 	bool		_rtl_time_actions_done{false};
 
 	FailureDetector	_failure_detector;
@@ -345,6 +345,7 @@ 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] {};
@@ -356,7 +357,6 @@ private:
 	Hysteresis	_offboard_available{false};
 
 	hrt_abstime	_last_print_mode_reject_time{0};	///< To remember when last notification was sent
-	bool            _mode_switch_mapped{false};
 
 	bool		_last_local_altitude_valid{false};
 	bool		_last_local_position_valid{false};
@@ -371,6 +371,7 @@ 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};
@@ -407,6 +408,7 @@ 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,6 +128,49 @@ 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
  *
@@ -850,23 +893,6 @@ 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

@@ -42,123 +42,6 @@
 
 using namespace time_literals;
 
-void FailureInjector::update()
-{
-	vehicle_command_s vehicle_command;
-
-	while (_vehicle_command_sub.update(&vehicle_command)) {
-		if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) {
-			continue;
-		}
-
-		bool handled = false;
-		bool supported = false;
-
-		const int failure_unit = static_cast<int>(vehicle_command.param1 + 0.5f);
-		const int failure_type = static_cast<int>(vehicle_command.param2 + 0.5f);
-		const int instance = static_cast<int>(vehicle_command.param3 + 0.5f);
-
-		if (failure_unit == vehicle_command_s::FAILURE_UNIT_SYSTEM_MOTOR) {
-			handled = true;
-
-			if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
-				PX4_INFO("CMD_INJECT_FAILURE, motors ok");
-				supported = false;
-
-				// 0 to signal all
-				if (instance == 0) {
-					supported = true;
-
-					for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
-						PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i);
-						_esc_blocked &= ~(1 << i);
-						_esc_wrong &= ~(1 << i);
-					}
-
-				} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
-					supported = true;
-
-					PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", instance - 1);
-					_esc_blocked &= ~(1 << (instance - 1));
-					_esc_wrong &= ~(1 << (instance - 1));
-				}
-			}
-
-			else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
-				PX4_WARN("CMD_INJECT_FAILURE, motors off");
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
-						PX4_INFO("CMD_INJECT_FAILURE, motor %d off", i);
-						_esc_blocked |= 1 << i;
-					}
-
-				} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
-					PX4_INFO("CMD_INJECT_FAILURE, motor %d off", instance - 1);
-					_esc_blocked |= 1 << (instance - 1);
-				}
-			}
-
-			else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
-				PX4_INFO("CMD_INJECT_FAILURE, motors wrong");
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
-						PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", i);
-						_esc_wrong |= 1 << i;
-					}
-
-				} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
-					PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", instance - 1);
-					_esc_wrong |= 1 << (instance - 1);
-				}
-			}
-		}
-
-		if (handled) {
-			vehicle_command_ack_s ack{};
-			ack.command = vehicle_command.command;
-			ack.from_external = false;
-			ack.result = supported ?
-				     vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED :
-				     vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
-			ack.timestamp = hrt_absolute_time();
-			_command_ack_pub.publish(ack);
-		}
-	}
-
-}
-
-void FailureInjector::manipulateEscStatus(esc_status_s &status)
-{
-	if (_esc_blocked != 0 || _esc_wrong != 0) {
-		unsigned offline = 0;
-
-		for (int i = 0; i < status.esc_count; i++) {
-			const unsigned i_esc = status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
-
-			if (_esc_blocked & (1 << i_esc)) {
-				unsigned function = status.esc[i].actuator_function;
-				memset(&status.esc[i], 0, sizeof(status.esc[i]));
-				status.esc[i].actuator_function = function;
-				offline |= 1 << i;
-
-			} else if (_esc_wrong & (1 << i_esc)) {
-				// Create wrong rerport for this motor by scaling key values up and down
-				status.esc[i].esc_voltage *= 0.1f;
-				status.esc[i].esc_current *= 0.1f;
-				status.esc[i].esc_rpm *= 10.0f;
-			}
-		}
-
-		status.esc_online_flags &= ~offline;
-	}
-}
-
 FailureDetector::FailureDetector(ModuleParams *parent) :
 	ModuleParams(parent)
 {
@@ -166,8 +49,6 @@ FailureDetector::FailureDetector(ModuleParams *parent) :
 
 bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode)
 {
-	_failure_injector.update();
-
 	failure_detector_status_u status_prev = _status;
 
 	if (vehicle_control_mode.flag_control_attitude_enabled) {
@@ -184,19 +65,8 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		_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);
-
-		if (_param_escs_en.get()) {
-			updateEscsStatus(vehicle_status, esc_status);
-		}
-
-		if (_param_fd_actuator_en.get()) {
-			updateMotorStatus(vehicle_status, esc_status);
-		}
+	if (_param_escs_en.get()) {
+		updateEscsStatus(vehicle_status);
 	}
 
 	if (_param_fd_imb_prop_thr.get() > 0) {
@@ -210,7 +80,7 @@ void FailureDetector::updateAttitudeStatus()
 {
 	vehicle_attitude_s attitude;
 
-	if (_vehicle_attitude_sub.update(&attitude)) {
+	if (_vehicule_attitude_sub.update(&attitude)) {
 
 		const matrix::Eulerf euler(matrix::Quatf(attitude.q));
 		const float roll(euler.phi());
@@ -257,26 +127,22 @@ void FailureDetector::updateExternalAtsStatus()
 	}
 }
 
-void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
+void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
 {
 	hrt_abstime time_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);
+		esc_status_s esc_status;
 
-		bool is_esc_failure = !is_all_escs_armed;
+		if (_esc_status_sub.update(&esc_status)) {
+			int all_escs_armed = (1 << esc_status.esc_count) - 1;
 
-		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(all_escs_armed != esc_status.esc_armed_flags, time_now);
 
-		_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;
+			if (_esc_failure_hysteresis.get_state()) {
+				_status.flags.arm_escs = true;
+			}
 		}
 
 	} else {
@@ -343,111 +209,3 @@ 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,13 +51,9 @@
 
 // subscriptions
 #include <uORB/Subscription.hpp>
-#include <uORB/Publication.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>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu_status.h>
 #include <uORB/topics/vehicle_status.h>
@@ -73,44 +69,26 @@ 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};
 };
 
 using uORB::SubscriptionData;
 
-class FailureInjector
-{
-public:
-	void update();
-
-	void manipulateEscStatus(esc_status_s &status);
-private:
-	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
-	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
-
-	uint32_t _esc_blocked{};
-	uint32_t _esc_wrong{};
-};
-
 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, const esc_status_s &esc_status);
-	void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
+	void updateEscsStatus(const vehicle_status_s &vehicle_status);
 	void updateImbalancedPropStatus();
 
 	failure_detector_status_u _status{};
@@ -125,20 +103,11 @@ private:
 	uint32_t _selected_accel_device_id{0};
 	hrt_abstime _imu_status_timestamp_prev{0};
 
-	// 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 _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
 	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)};
-
-	FailureInjector _failure_injector;
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@@ -148,12 +117,6 @@ 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,
-
-		// 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
+		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
 	)
 };

src/modules/commander/failure_detector/failure_detector_params.c

@@ -156,59 +156,3 @@ 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,6 +383,9 @@ 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);
@@ -431,7 +434,10 @@ 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 (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		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)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else if (status_flags.vtol_transition_failure) {
@@ -469,7 +475,10 @@ 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 (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		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)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 		} else {
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@@ -479,9 +488,12 @@ 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 (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		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)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else {
@@ -491,7 +503,15 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		break;
 
 	case commander_state_s::MAIN_STATE_ORBIT:
-		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		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)) {
 			// 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
@@ -528,9 +548,12 @@ 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 (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+		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)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else if (status_flags.vtol_transition_failure) {
@@ -567,10 +590,12 @@ 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 (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+		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)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else {
@@ -581,9 +606,12 @@ 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 (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+		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)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else {

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -98,15 +98,6 @@ public:
 	 */
 	virtual void allocate() = 0;
 
-	/**
-	 * Set actuator failure flag
-	 * This prevents a change of the scaling in the matrix normalization step
-	 * in case of a motor failure.
-	 *
-	 * @param failure  Motor failure flag
-	 */
-	void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; }
-
 	/**
 	 * Set the control effectiveness matrix
 	 *
@@ -243,5 +234,4 @@ protected:
 	matrix::Vector<float, NUM_AXES> _control_trim; 		///< Control at trim actuator values
 	int _num_actuators{0};
 	bool _normalize_rpy{false};				///< if true, normalize roll, pitch and yaw columns
-	bool _had_actuator_failure{false};
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -59,7 +59,7 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 	if (_mix_update_needed) {
 		matrix::geninv(_effectiveness, _mix);
 
-		if (_normalization_needs_update && !_had_actuator_failure) {
+		if (_normalization_needs_update) {
 			updateControlAllocationMatrixScale();
 			_normalization_needs_update = false;
 		}

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -92,8 +92,8 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act
 	float k_max = 0.f;
 
 	for (int i = 0; i < _num_actuators; i++) {
-		// Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains
-		if (fabsf(desaturation_vector(i)) < 0.2f) {
+		// Avoid division by zero. If desaturation_vector(i) is zero, there's nothing we can do to unsaturate anyway
+		if (fabsf(desaturation_vector(i)) < FLT_EPSILON) {
 			continue;
 		}
 

src/modules/control_allocator/ControlAllocator.cpp

@@ -205,7 +205,7 @@ ControlAllocator::update_allocation_method(bool force)
 bool
 ControlAllocator::update_effectiveness_source()
 {
-	const EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
+	EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
 
 	if (_effectiveness_source_id != source) {
 
@@ -305,12 +305,8 @@ ControlAllocator::Run()
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
 
-		if (_handled_motor_failure_bitmask == 0) {
-			// We don't update the geometry after an actuator failure, as it could lead to unexpected results
-			// (e.g. a user could add/remove motors, such that the bitmask isn't correct anymore)
-			updateParams();
-			parameters_updated();
-		}
+		updateParams();
+		parameters_updated();
 	}
 
 	if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) {
@@ -380,8 +376,6 @@ ControlAllocator::Run()
 	if (do_update) {
 		_last_run = now;
 
-		check_for_motor_failures();
-
 		update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
 
 		// Set control setpoint vector(s)
@@ -509,27 +503,6 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
 			}
 		}
 
-		// Handle failed actuators
-		if (_handled_motor_failure_bitmask) {
-			actuator_idx = 0;
-			memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix));
-
-			for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) {
-				int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
-
-				if (_handled_motor_failure_bitmask & (1 << motors_idx)) {
-					ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix];
-
-					for (int i = 0; i < NUM_AXES; i++) {
-						matrix(i, actuator_idx_matrix[selected_matrix]) = 0.0f;
-					}
-				}
-
-				++actuator_idx_matrix[selected_matrix];
-				++actuator_idx;
-			}
-		}
-
 		for (int i = 0; i < _num_control_allocation; ++i) {
 			_control_allocation[i]->setActuatorMin(minimum[i]);
 			_control_allocation[i]->setActuatorMax(maximum[i]);
@@ -584,8 +557,7 @@ ControlAllocator::publish_control_allocator_status()
 	control_allocator_status.allocated_thrust[2] = allocated_control(5);
 
 	// Unallocated control
-	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[0]->getControlSetpoint() -
-			allocated_control;
+	matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[0]->getControlSetpoint() - allocated_control;
 	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
 	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
 	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
@@ -613,9 +585,6 @@ ControlAllocator::publish_control_allocator_status()
 		}
 	}
 
-	// Handled motor failures
-	control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask;
-
 	_control_allocator_status_pub.publish(control_allocator_status);
 }
 
@@ -635,7 +604,7 @@ ControlAllocator::publish_actuator_controls()
 	int actuator_idx = 0;
 	int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
 
-	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask;
+	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors();
 
 	// motors
 	int motors_idx;
@@ -679,56 +648,6 @@ ControlAllocator::publish_actuator_controls()
 	}
 }
 
-void
-ControlAllocator::check_for_motor_failures()
-{
-	failure_detector_status_s failure_detector_status;
-
-	if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE
-	    && _failure_detector_status_sub.update(&failure_detector_status)) {
-		if (failure_detector_status.fd_motor) {
-
-			if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) {
-				// motor failure bitmask changed
-				switch ((FailureMode)_param_ca_failure_mode.get()) {
-				case FailureMode::REMOVE_FIRST_FAILING_MOTOR: {
-						// Count number of failed motors
-						const int num_motors_failed = math::countSetBits(failure_detector_status.motor_failure_mask);
-
-						// Only handle if it is the first failure
-						if (_handled_motor_failure_bitmask == 0 && num_motors_failed == 1) {
-							_handled_motor_failure_bitmask = failure_detector_status.motor_failure_mask;
-							PX4_WARN("Removing motor from allocation (0x%x)", _handled_motor_failure_bitmask);
-
-							for (int i = 0; i < _num_control_allocation; ++i) {
-								_control_allocation[i]->setHadActuatorFailure(true);
-							}
-
-							update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE);
-						}
-					}
-					break;
-
-				default:
-					break;
-				}
-
-			}
-
-		} else if (_handled_motor_failure_bitmask != 0) {
-			// Clear bitmask completely
-			PX4_INFO("Restoring all motors");
-			_handled_motor_failure_bitmask = 0;
-
-			for (int i = 0; i < _num_control_allocation; ++i) {
-				_control_allocation[i]->setHadActuatorFailure(false);
-			}
-
-			update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE);
-		}
-	}
-}
-
 int ControlAllocator::task_spawn(int argc, char *argv[])
 {
 	ControlAllocator *instance = new ControlAllocator();
@@ -797,11 +716,6 @@ int ControlAllocator::print_status()
 		PX4_INFO("  Configured actuators: %i", _control_allocation[i]->numConfiguredActuators());
 	}
 
-	if (_handled_motor_failure_bitmask) {
-		PX4_INFO("Failed motors: %i (0x%x)", math::countSetBits(_handled_motor_failure_bitmask),
-			 _handled_motor_failure_bitmask);
-	}
-
 	// Print perf
 	perf_print_counter(_loop_perf);
 

src/modules/control_allocator/ControlAllocator.hpp

@@ -74,7 +74,6 @@
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/failure_detector_status.h>
 
 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
 {
@@ -129,8 +128,6 @@ private:
 
 	void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);
 
-	void check_for_motor_failures();
-
 	void publish_control_allocator_status();
 
 	void publish_actuator_controls();
@@ -155,11 +152,6 @@ private:
 		HELICOPTER = 10,
 	};
 
-	enum class FailureMode {
-		IGNORE = 0,
-		REMOVE_FIRST_FAILING_MOTOR = 1,
-	};
-
 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
 	ActuatorEffectiveness *_actuator_effectiveness{nullptr}; 	///< class providing actuator effectiveness
 
@@ -183,15 +175,10 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
 
 	matrix::Vector3f _torque_sp;
 	matrix::Vector3f _thrust_sp;
 
-	// Reflects motor failures that are currently handled, not motor failures that are reported.
-	// For example, the system might report two motor failures, but only the first one is handled by CA
-	uint16_t _handled_motor_failure_bitmask{0};
-
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};
@@ -206,7 +193,6 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
 		(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
-		(ParamInt<px4::params::CA_FAILURE_MODE>) _param_ca_failure_mode,
 		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
 	)
 

src/modules/control_allocator/module.yaml

@@ -436,19 +436,6 @@ parameters:
             max: 1
             default: [0.05, 0.15, 0.25, 0.35, 0.45]
 
-        # Others
-        CA_FAILURE_MODE:
-            description:
-                short: Motor failure handling mode
-                long: |
-                    This is used to specify how to handle motor failures
-                    reported by failure detector.
-            type: enum
-            values:
-                0: Ignore
-                1: Remove first failed motor from effectiveness
-            default: 0
-
 # Mixer
 mixer:
     actuator_types:

src/modules/ekf2/EKF2.cpp

@@ -1878,9 +1878,6 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 
 void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 {
-	_accel_cal.cal_available = true;
-	return;
-
 	if (_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS) {
 		_accel_cal.cal_available = false;
 		return;
@@ -1921,9 +1918,6 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 
 void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 {
-	_gyro_cal.cal_available = true;
-	return;
-
 	static constexpr float max_var_allowed = 1e-3f;
 	static constexpr float max_var_ratio = 1e2f;
 
@@ -1954,9 +1948,6 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
-	_mag_cal.cal_available = true;
-	return;
-
 	// Check if conditions are OK for learning of magnetometer bias values
 	// the EKF is operating in the correct mode and there are no filter faults
 

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

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

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -2682,6 +2682,12 @@ 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/land_detector/LandDetector.cpp

@@ -93,15 +93,6 @@ void LandDetector::Run()
 	actuator_armed_s actuator_armed;
 
 	if (_actuator_armed_sub.update(&actuator_armed)) {
-		if (!_armed && actuator_armed.armed) {
-			// force in air briefly
-			_freefall_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
-			_ground_contact_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
-			_maybe_landed_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
-			_landed_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
-			_ground_effect_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
-		}
-
 		_armed = actuator_armed.armed;
 	}
 

src/modules/logger/log_writer_file.cpp

@@ -287,9 +287,7 @@ int LogWriterFile::hardfault_store_filename(const char *log_file)
 
 void LogWriterFile::stop_log(LogType type)
 {
-	lock();
 	_buffers[(int)type]._should_run = false;
-	unlock();
 	notify();
 }
 
@@ -314,10 +312,8 @@ int LogWriterFile::thread_start()
 void LogWriterFile::thread_stop()
 {
 	// this will terminate the main loop of the writer thread
-	lock();
-	_exit_thread.store(true);
+	_exit_thread = true;
 	_buffers[0]._should_run = _buffers[1]._should_run = false;
-	unlock();
 
 	notify();
 
@@ -339,10 +335,10 @@ void *LogWriterFile::run_helper(void *context)
 
 void LogWriterFile::run()
 {
-	while (!_exit_thread.load()) {
+	while (!_exit_thread) {
 		// Outer endless loop
 		// Wait for _should_run flag
-		while (!_exit_thread.load()) {
+		while (!_exit_thread) {
 			bool start = false;
 			pthread_mutex_lock(&_mtx);
 			pthread_cond_wait(&_cv, &_mtx);
@@ -354,7 +350,7 @@ void LogWriterFile::run()
 			}
 		}
 
-		if (_exit_thread.load()) {
+		if (_exit_thread) {
 			break;
 		}
 
@@ -484,7 +480,7 @@ void LogWriterFile::run()
 			 * not an issue because notify() is called regularly.
 			 * If the logger was switched off in the meantime, do not wait for data, instead run this loop
 			 * once more to write remaining data and close the file. */
-			if (_buffers[0]._should_run || _buffers[1]._should_run) {
+			if (_buffers[0]._should_run) {
 				pthread_cond_wait(&_cv, &_mtx);
 			}
 		}

src/modules/logger/log_writer_file.h

@@ -34,7 +34,6 @@
 #pragma once
 
 #include <px4_platform_common/defines.h>
-#include <px4_platform_common/atomic.h>
 #include <stdint.h>
 #include <pthread.h>
 #include <drivers/drv_hrt.h>
@@ -206,8 +205,8 @@ private:
 
 	LogFileBuffer _buffers[(int)LogType::Count];
 
-	px4::atomic_bool	_exit_thread{false};
-	bool			_need_reliable_transfer{false};
+	bool 		_exit_thread = false;
+	bool		_need_reliable_transfer = false;
 	pthread_mutex_t		_mtx;
 	pthread_cond_t		_cv;
 	pthread_t _thread = 0;

src/modules/manual_control/ManualControl.cpp

@@ -32,9 +32,6 @@
  ****************************************************************************/
 
 #include "ManualControl.hpp"
-
-#include <px4_platform_common/events.h>
-#include <lib/systemlib/mavlink_log.h>
 #include <uORB/topics/commander_state.h>
 #include <uORB/topics/vehicle_command.h>
 
@@ -67,16 +64,6 @@ void ManualControl::Run()
 	perf_begin(_loop_perf);
 	perf_count(_loop_interval_perf);
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_system_id = vehicle_status.system_id;
-			_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
-			_vtol = vehicle_status.is_vtol;
-		}
-	}
-
 	// Check if parameters have changed
 	if (_parameter_update_sub.updated()) {
 		// clear update
@@ -91,53 +78,6 @@ void ManualControl::Run()
 
 		_selector.setRcInMode(_param_com_rc_in_mode.get());
 		_selector.setTimeout(_param_com_rc_loss_t.get() * 1_s);
-
-		// MAN_ARM_GESTURE
-		if (_param_man_arm_gesture.get() == 1) {
-			// RC_MAP_ARM_SW & MAN_ARM_GESTURE: disable arm gesture if an arm switch is configured
-			param_t param_rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
-
-			if (param_rc_map_arm_sw != PARAM_INVALID) {
-				int32_t rc_map_arm_sw = 0;
-				param_get(param_rc_map_arm_sw, &rc_map_arm_sw);
-
-				if (rc_map_arm_sw > 0) {
-					_param_man_arm_gesture.set(0); // disable arm gesture
-					_param_man_arm_gesture.commit();
-
-					orb_advert_t mavlink_log_pub = nullptr;
-					mavlink_log_critical(&mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t")
-					/* EVENT
-					* @description <param>MAN_ARM_GESTURE</param> is now set to disable arm/disarm stick gesture.
-					*/
-					events::send(events::ID("rc_update_arm_stick_gesture_disabled_with_switch"), {events::Log::Info, events::LogInternal::Disabled},
-						     "Arm stick gesture disabled if arm switch in use");
-				}
-			}
-
-			// MC_AIRMODE & MAN_ARM_GESTURE: check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture
-			if ((_param_man_arm_gesture.get() == 1) && (_rotary_wing || _vtol)) {
-				param_t param_mc_airmode = param_find("MC_AIRMODE");
-
-				if (param_mc_airmode != PARAM_INVALID) {
-					int32_t airmode = 0;
-					param_get(param_mc_airmode, &airmode);
-
-					if (airmode == 2) {
-						airmode = 1; // change to roll/pitch airmode
-						param_set(param_mc_airmode, &airmode);
-
-						orb_advert_t mavlink_log_pub = nullptr;
-						mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t")
-						/* EVENT
-						* @description <param>MC_AIRMODE</param> is now set to roll/pitch airmode.
-						*/
-						events::send(events::ID("commander_airmode_requires_no_arm_gesture"), {events::Log::Error, events::LogInternal::Disabled},
-							     "Yaw Airmode requires disabling the stick arm gesture");
-					}
-				}
-			}
-		}
 	}
 
 	const hrt_abstime now = hrt_absolute_time();
@@ -421,7 +361,7 @@ void ManualControl::send_camera_mode_command(CameraMode camera_mode)
 	vehicle_command_s command{};
 	command.command = vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE;
 	command.param2 = static_cast<float>(camera_mode);
-	command.target_system = _system_id;
+	command.target_system = _param_mav_sys_id.get();
 	command.target_component = 100; // any camera
 
 	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
@@ -435,7 +375,7 @@ void ManualControl::send_photo_command()
 	command.command = vehicle_command_s::VEHICLE_CMD_IMAGE_START_CAPTURE;
 	command.param3 = 1; // one picture
 	command.param4 = _image_sequence++;
-	command.target_system = _system_id;
+	command.target_system = _param_mav_sys_id.get();
 	command.target_component = 100; // any camera
 
 	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
@@ -455,7 +395,7 @@ void ManualControl::send_video_command()
 		command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_START_CAPTURE;
 	}
 
-	command.target_system = _system_id;
+	command.target_system = _param_mav_sys_id.get();
 	command.target_component = 100; // any camera
 
 	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};

src/modules/manual_control/ManualControl.hpp

@@ -46,7 +46,6 @@
 #include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/Publication.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/SubscriptionCallback.hpp>
@@ -98,8 +97,6 @@ private:
 	uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
 	int _previous_manual_control_input_instance{-1};
 	uORB::SubscriptionCallbackWorkItem _manual_control_setpoint_subs[MAX_MANUAL_INPUT_COUNT] {
 		{this, ORB_ID(manual_control_input), 0},
@@ -140,13 +137,10 @@ private:
 		(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
 		(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
 		(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
-		(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6
+		(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6,
+		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id
 	)
 
 	unsigned _image_sequence {0};
 	bool _video_recording {false}; // TODO: hopefully there is a command soon to toggle without keeping state
-
-	uint8_t _system_id{1};
-	bool _rotary_wing{false};
-	bool _vtol{false};
 };

src/modules/mavlink/mavlink_messages.cpp

@@ -260,6 +260,11 @@ 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,6 +131,9 @@ 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.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) {
+			if (status.engine_failure) {
 				msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
 			}
 

src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp

@@ -141,6 +141,7 @@ 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,6 +48,7 @@ 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

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   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,6 +1,6 @@
-/****************************************************************************
+/***************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -30,35 +30,44 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+/**
+ * @file enginefailure.h
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
 
 #pragma once
 
-#include "autopilot_tester.h"
+#include "navigator_mode.h"
+#include "mission_block.h"
 
-#include <mavsdk/mavsdk.h>
-#include <mavsdk/plugins/action/action.h>
-#include <mavsdk/plugins/failure/failure.h>
+class Navigator;
 
-
-class AutopilotTesterFailure : public AutopilotTester
+class EngineFailure : public MissionBlock
 {
 public:
-	AutopilotTesterFailure() = default;
-	~AutopilotTesterFailure() = default;
+	EngineFailure(Navigator *navigator);
+	~EngineFailure() = default;
 
-	void set_param_sys_failure_en(bool value);
-	void set_param_fd_act_en(bool value);
-	void set_param_mc_airmode(int value);
-	void set_param_ca_failure_mode(int value);
-	void set_param_com_act_fail_act(int value);
-
-	void enable_actuator_output_status();
-	void ensure_motor_stopped(unsigned index, unsigned num_motors);
-
-	void inject_failure(mavsdk::Failure::FailureUnit failure_unit, mavsdk::Failure::FailureType failure_type, int instance,
-			    mavsdk::Failure::Result expected_result);
-	void connect(const std::string uri);
+	void on_inactive() override;
+	void on_activation() override;
+	void on_active() override;
 
 private:
-	std::unique_ptr<mavsdk::Failure> _failure{};
+	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,6 +41,7 @@
 
 #pragma once
 
+#include "enginefailure.h"
 #include "follow_target.h"
 #include "geofence.h"
 #include "land.h"
@@ -85,7 +86,7 @@ using namespace time_literals;
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 8
+#define NAVIGATOR_MODE_ARRAY_SIZE 9
 
 class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
@@ -392,6 +393,7 @@ 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,17 +78,19 @@ 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] = &_takeoff;
-	_navigation_mode_array[4] = &_land;
-	_navigation_mode_array[5] = &_precland;
-	_navigation_mode_array[6] = &_vtol_takeoff;
-	_navigation_mode_array[7] = &_follow_target;
+	_navigation_mode_array[3] = &_engineFailure;
+	_navigation_mode_array[4] = &_takeoff;
+	_navigation_mode_array[5] = &_land;
+	_navigation_mode_array[6] = &_precland;
+	_navigation_mode_array[7] = &_vtol_takeoff;
+	_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");
@@ -695,6 +697,11 @@ 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;

src/modules/rc_update/params.c

@@ -1557,37 +1557,98 @@ PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0);
 
 /**
- * Button flight mode selection
+ * Mask to indicate if the channel is a button for the trigger action
  *
- * This bitmask allows to specify multiple channels for changing flight modes using
- * momentary buttons. Each channel is assigned to a mode slot ((lowest channel = slot 1).
- * The resulting modes for each slot X is defined by the COM_FLTMODEX parameters.
- * The functionality can be used only if RC_MAP_FLTMODE is disabled.
+ * This bitmask allows to specify multiple channels to indicate whether it's a button.
+ * If not marked, the channel is assumed to be a switch. This configuration allows the
+ * rc_update to correctly decode the input signals to trigger appropriate actions.
  *
- * The maximum number of available slots and hence bits set in the mask is 6.
  * @min 0
  * @max 258048
- * @group Radio Switches
- * @bit 0 Mask Channel 1 as a mode button
- * @bit 1 Mask Channel 2 as a mode button
- * @bit 2 Mask Channel 3 as a mode button
- * @bit 3 Mask Channel 4 as a mode button
- * @bit 4 Mask Channel 5 as a mode button
- * @bit 5 Mask Channel 6 as a mode button
- * @bit 6 Mask Channel 7 as a mode button
- * @bit 7 Mask Channel 8 as a mode button
- * @bit 8 Mask Channel 9 as a mode button
- * @bit 9 Mask Channel 10 as a mode button
- * @bit 10 Mask Channel 11 as a mode button
- * @bit 11 Mask Channel 12 as a mode button
- * @bit 12 Mask Channel 13 as a mode button
- * @bit 13 Mask Channel 14 as a mode button
- * @bit 14 Mask Channel 15 as a mode button
- * @bit 15 Mask Channel 16 as a mode button
- * @bit 16 Mask Channel 17 as a mode button
- * @bit 17 Mask Channel 18 as a mode button
+ * @group Radio Trigger
+ * @bit 0 Mask Channel 1 as a button
+ * @bit 1 Mask Channel 2 as a button
+ * @bit 2 Mask Channel 3 as a button
+ * @bit 3 Mask Channel 4 as a button
+ * @bit 4 Mask Channel 5 as a button
+ * @bit 5 Mask Channel 6 as a button
+ * @bit 6 Mask Channel 7 as a button
+ * @bit 7 Mask Channel 8 as a button
+ * @bit 8 Mask Channel 9 as a button
+ * @bit 9 Mask Channel 10 as a button
+ * @bit 10 Mask Channel 11 as a button
+ * @bit 11 Mask Channel 12 as a button
+ * @bit 12 Mask Channel 13 as a button
+ * @bit 13 Mask Channel 14 as a button
+ * @bit 14 Mask Channel 15 as a button
+ * @bit 15 Mask Channel 16 as a button
+ * @bit 16 Mask Channel 17 as a button
+ * @bit 17 Mask Channel 18 as a button
  */
-PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0);
+PARAM_DEFINE_INT32(RC_TRIG_BTN_MASK, 0);
+
+/**
+ * RC Channel for trigger slot 1
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Trigger
+ * @value 0 Unassigned
+ * @value 1 Channel 1
+ * @value 2 Channel 2
+ * @value 3 Channel 3
+ * @value 4 Channel 4
+ * @value 5 Channel 5
+ * @value 6 Channel 6
+ * @value 7 Channel 7
+ * @value 8 Channel 8
+ * @value 9 Channel 9
+ * @value 10 Channel 10
+ * @value 11 Channel 11
+ * @value 12 Channel 12
+ * @value 13 Channel 13
+ * @value 14 Channel 14
+ * @value 15 Channel 15
+ * @value 16 Channel 16
+ * @value 17 Channel 17
+ * @value 18 Channel 18
+ */
+PARAM_DEFINE_INT32(RC_TRIG1_CHAN, 0);
+
+/**
+ * Which action the Trigger slot 1 triggers
+ *
+ * The flight mode enums follow the convention defined in the commander_state.msg uORB message.
+ * Everything else (16 ~ ) is a non-flight-mode actions
+ *
+ * @min -1
+ * @max 20
+ * @group Radio Trigger
+ * @value -1 Unassigned
+ * @value 0 Manual
+ * @value 1 Altitude
+ * @value 2 Position
+ * @value 3 Mission
+ * @value 4 Loiter
+ * @value 5 Return
+ * @value 6 Acro
+ * @value 7 Offboard
+ * @value 8 Stabilized
+ * @value 10 Takeoff
+ * @value 11 Land
+ * @value 12 Follow Me
+ * @value 13 Precision Land
+ * @value 14 Orbit
+ * @value 15 Auto VTOL Takeoff
+ * @value 16 Killswitch
+ * @value 17 Arm
+ * @value 18 VTOL Transition
+ * @value 19 Gear
+ * @value 20 Photo
+ * @value 21 Video
+ */
+PARAM_DEFINE_INT32(RC_TRIG1_ACTION, 0);
+
 
 /**
  * AUX1 Passthrough RC channel

src/modules/rc_update/rc_update.cpp

@@ -64,7 +64,15 @@ static bool operator !=(const manual_control_switches_s &a, const manual_control
 
 RCUpdate::RCUpdate() :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	_trigger_slots_hysteresis{
+	systemlib::Hysteresis{false},
+	systemlib::Hysteresis{false},
+	systemlib::Hysteresis{false},
+	systemlib::Hysteresis{false},
+	systemlib::Hysteresis{false},
+	systemlib::Hysteresis{false}
+}
 {
 	// initialize parameter handles
 	for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) {
@@ -99,10 +107,19 @@ RCUpdate::RCUpdate() :
 		_parameter_handles.rc_map_param[i] = param_find(name);
 	}
 
+	// Find param handles for Generic Trigger Channel & Actions for slot 1 ~ 6
+	for (uint8_t trig_slot = 1; trig_slot <= RC_TRIG_SLOT_COUNT; trig_slot++) {
+		char param_name_buf[17] = {};
+		snprintf(param_name_buf, sizeof(param_name_buf), "RC_TRIG_%d_CHAN", trig_slot);
+		_parameter_handles.generic_trigger_chan[trig_slot - 1] = param_find(param_name_buf);
+		snprintf(param_name_buf, sizeof(param_name_buf), "RC_TRIG_%d_ACTION", trig_slot);
+		_parameter_handles.generic_trigger_action[trig_slot - 1] = param_find(param_name_buf);
+	}
+
 	rc_parameter_map_poll(true /* forced */);
 	parameters_updated();
 
-	_button_pressed_hysteresis.set_hysteresis_time_from(false, 50_ms);
+	//_button_pressed_hysteresis.set_hysteresis_time_from(false, 50_ms);
 }
 
 RCUpdate::~RCUpdate()
@@ -154,6 +171,16 @@ void RCUpdate::parameters_updated()
 
 	update_rc_functions();
 
+	// Update and check values of the generic action parameters
+	for (uint8_t trig_slot = 1; trig_slot <= RC_TRIG_SLOT_COUNT; trig_slot++) {
+		int32_t new_channel{RC_TRIG_CHAN_UNASSIGNED};
+		int32_t new_action{RC_TRIG_ACTION_UNASSIGNED};
+		param_get(_parameter_handles.generic_trigger_chan[trig_slot - 1], &new_channel);
+		param_get(_parameter_handles.generic_trigger_action[trig_slot - 1], &new_action);
+
+
+	}
+
 	// deprecated parameters, will be removed post v1.12 once QGC is updated
 	{
 		int32_t rc_map_value = 0;
@@ -230,8 +257,6 @@ void RCUpdate::update_rc_functions()
 	for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
 		_rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
 	}
-
-	map_flight_modes_buttons();
 }
 
 void RCUpdate::rc_parameter_map_poll(bool forced)
@@ -306,42 +331,6 @@ void RCUpdate::set_params_from_rc()
 	}
 }
 
-void
-RCUpdate::map_flight_modes_buttons()
-{
-	static_assert(rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + manual_control_switches_s::MODE_SLOT_NUM <= sizeof(
-			      _rc.function) / sizeof(_rc.function[0]), "Unexpected number of RC functions");
-	static_assert(rc_channels_s::FUNCTION_FLTBTN_SLOT_COUNT == manual_control_switches_s::MODE_SLOT_NUM,
-		      "Unexpected number of Flight Modes slots");
-
-	// Reset all the slots to -1
-	for (uint8_t slot = 0; slot < manual_control_switches_s::MODE_SLOT_NUM; slot++) {
-		_rc.function[rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot] = -1;
-	}
-
-	// If the functionality is disabled we don't need to map channels
-	const int flightmode_buttons = _param_rc_map_flightmode_buttons.get();
-
-	if (flightmode_buttons == 0) {
-		return;
-	}
-
-	uint8_t slot = 0;
-
-	for (uint8_t channel = 0; channel < RC_MAX_CHAN_COUNT; channel++) {
-		if (flightmode_buttons & (1 << channel)) {
-			PX4_DEBUG("Slot %d assigned to channel %d", slot + 1, channel);
-			_rc.function[rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot] = channel;
-			slot++;
-		}
-
-		if (slot >= manual_control_switches_s::MODE_SLOT_NUM) {
-			// we have filled all the available slots
-			break;
-		}
-	}
-}
-
 void RCUpdate::Run()
 {
 	if (should_exit()) {
@@ -605,7 +594,45 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 			switches.mode_slot = num_slots;
 		}
 
-	} else if (_param_rc_map_flightmode_buttons.get() > 0) {
+	}
+
+	// Use the Generic RC Switch / Button only when the RC is in use
+	if (_manual_control_setpoint_sub.update() &&
+	    _manual_control_setpoint_sub.get().data_source == manual_control_setpoint_s::SOURCE_RC) {
+		_manual_control_setpoint_source_is_rc = true;
+	}
+
+	// Go through the trigger slots and update the states
+	const uint32_t rc_trigger_is_button_mask = _param_rc_trig_btn_mask.get();
+
+	for (uint8_t trig_slot = 1; trig_slot <= RC_TRIG_SLOT_COUNT; trig_slot++) {
+		int channel{RC_TRIG_CHAN_UNASSIGNED};
+		param_get(_trigger_channel_param_handles[trig_slot - 1], &channel);
+		int action{RC_TRIGGER_ACTION_UNASSIGNED};
+		param_get(_trigger_action_param_handles[trig_slot - 1], &action);
+
+		if (channel > RC_TRIG_CHAN_UNASSIGNED) {
+
+		}
+	}
+
+	// Update the Generic Action states
+	const uint8_t trig1_chan = _param_rc_trig1_chan.get();
+	const uint8_t trig1_action = _param_rc_trig1_action.get();
+
+	// Trigger Channel is configured
+	if (trig1_chan > 0) {
+		const bool is_btn = _param_rc_trig_btn_mask.get() & (1 << 0);
+
+		if (is_btn) {
+
+		} else {
+			// Is Switch
+
+		}
+	}
+
+	else if (_param_rc_map_flightmode_buttons.get() > 0) {
 		switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE;
 		bool is_consistent_button_press = false;
 
@@ -628,12 +655,6 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 				break;
 			}
 		}
-
-		_button_pressed_hysteresis.set_state_and_update(is_consistent_button_press, hrt_absolute_time());
-
-		if (_button_pressed_hysteresis.get_state()) {
-			switches.mode_slot = _potential_button_press_slot;
-		}
 	}
 
 	switches.return_switch     = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN,     _param_rc_return_th.get());

src/modules/rc_update/rc_update.h

@@ -48,6 +48,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
+
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
@@ -66,6 +67,43 @@ using namespace time_literals;
 namespace rc_update
 {
 
+// Number of Generic Trigger slots that can be configured
+static constexpr uint8_t RC_TRIG_SLOT_COUNT = 6;
+
+// Value of the RC_TRIG#_CHAN when the channel is unassigned
+static constexpr uint8_t RC_TRIG_CHAN_UNASSIGNED = 0;
+
+// Value of the RC_TRIG#_ACTION when the action is unassigned
+static constexpr uint8_t RC_TRIG_ACTION_UNASSIGNED = -1;
+
+// Enum class translation of the RC_TRIG#_ACTION values
+enum RC_TRIGGER_ACTIONS {
+	RC_TRIGGER_ACTION_UNASSIGNED = -1,
+	// Commander States (defined in commander_state.msg)
+	RC_TRIGGER_ACTION_MANUAL_FLIGHTMODE = 0,
+	RC_TRIGGER_ACTION_ALTITUDE_FLIGHTMODE = 1,
+	RC_TRIGGER_ACTION_POSITION_FLIGHTMODE = 2,
+	RC_TRIGGER_ACTION_MISSION_FLIHGTMODE = 3,
+	RC_TRIGGER_ACTION_HOLD_FLIGHTMODE = 4,
+	RC_TRIGGER_ACTION_RETURN_FLIGHTMODE = 5,
+	RC_TRIGGER_ACTION_ACRO_FLIGHTMODE = 6,
+	RC_TRIGGER_ACTION_OFFBOARD_FLIGHTMODE = 7,
+	RC_TRIGGER_ACTION_STABILIZED_FLIGHTMODE = 8,
+	RC_TRIGGER_ACTION_TAKEOFF = 10,
+	RC_TRIGGER_ACTION_LAND = 11,
+	RC_TRIGGER_ACTION_FOLLOW_ME_FLIGHTMODE = 12,
+	RC_TRIGGER_ACTION_PRECISION_LAND_FLIGHTMODE = 13,
+	RC_TRIGGER_ACTION_ORBIT_FLIGHTMODE = 14,
+	RC_TRIGGER_ACTION_AUTO_VTOL_TAKEOFF = 15,
+	// Non- Commander State Actions
+	RC_TRIGGER_ACTION_KILLSWITCH = 16,
+	RC_TRIGGER_ACTION_ARM = 17,
+	RC_TRIGGER_ACTION_VTOL_TRANSITION = 18,
+	RC_TRIGGER_ACTION_GEAR = 19,
+	RC_TRIGGER_ACTION_PHOTO = 20,
+	RC_TRIGGER_ACTION_VIDEO = 21
+};
+
 /**
  ** class RCUpdate
  *
@@ -132,8 +170,6 @@ private:
 	 */
 	void		set_params_from_rc();
 
-	void		map_flight_modes_buttons();
-
 	static constexpr uint8_t RC_MAX_CHAN_COUNT{input_rc_s::RC_INPUT_MAX_CHANNELS}; /**< maximum number of r/c channels we handle */
 
 	struct Parameters {
@@ -144,6 +180,9 @@ private:
 		bool rev[RC_MAX_CHAN_COUNT];
 
 		int32_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
+
+		uint8_t generic_trigger_chan[RC_TRIG_SLOT_COUNT];
+		uint8_t generic_trigger_action[RC_TRIG_SLOT_COUNT];
 	} _parameters{};
 
 	struct ParameterHandles {
@@ -154,16 +193,17 @@ private:
 		param_t dz[RC_MAX_CHAN_COUNT];
 
 		param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
-		param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];	/**< param handles for the parameters which are bound
-								to a RC channel, equivalent float values in the
-								_parameters struct are not existing
-								because these parameters are never read. */
+		param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
+		/**< param handles for the parameters which are bound to a RC channel, equivalent float values
+		 * in the_parameters struct are not existing because these parameters are never read. */
+
+		param_t generic_trigger_chan[RC_TRIG_SLOT_COUNT];
+		param_t generic_trigger_action[RC_TRIG_SLOT_COUNT];
 	} _parameter_handles{};
 
 	uORB::SubscriptionCallbackWorkItem _input_rc_sub{this, ORB_ID(input_rc)};
-
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
+	uORB::SubscriptionData<manual_control_setpoint_s> _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)};
 	uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
 
@@ -190,8 +230,12 @@ private:
 	uint8_t _channel_count_previous{0};
 	uint8_t _input_source_previous{input_rc_s::RC_INPUT_SOURCE_UNKNOWN};
 
-	uint8_t _potential_button_press_slot{0};
-	systemlib::Hysteresis _button_pressed_hysteresis{false};
+	// Flag to indicate that RC input is being used for manual control (whether we can use generic action)
+	bool _manual_control_setpoint_source_is_rc{false};
+	systemlib::Hysteresis _trigger_slots_hysteresis[RC_TRIG_SLOT_COUNT];
+	param_t _trigger_channel_param_handles[RC_TRIG_SLOT_COUNT] {};
+	param_t _trigger_action_param_handles[RC_TRIG_SLOT_COUNT] {};
+
 	systemlib::Hysteresis _rc_signal_lost_hysteresis{true};
 
 	uint8_t _channel_count_max{0};
@@ -201,17 +245,14 @@ private:
 	perf_counter_t _valid_data_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": valid data interval")};
 
 	DEFINE_PARAMETERS(
-
 		(ParamInt<px4::params::RC_MAP_ROLL>) _param_rc_map_roll,
 		(ParamInt<px4::params::RC_MAP_PITCH>) _param_rc_map_pitch,
 		(ParamInt<px4::params::RC_MAP_YAW>) _param_rc_map_yaw,
 		(ParamInt<px4::params::RC_MAP_THROTTLE>) _param_rc_map_throttle,
 		(ParamInt<px4::params::RC_MAP_FAILSAFE>) _param_rc_map_failsafe,
-
 		(ParamInt<px4::params::RC_MAP_FLTMODE>) _param_rc_map_fltmode,
-
+		(ParamInt<px4::params::RC_TRIG_BTN_MASK>) _param_rc_trig_btn_mask,
 		(ParamInt<px4::params::RC_MAP_FLAPS>) _param_rc_map_flaps,
-
 		(ParamInt<px4::params::RC_MAP_RETURN_SW>) _param_rc_map_return_sw,
 		(ParamInt<px4::params::RC_MAP_LOITER_SW>) _param_rc_map_loiter_sw,
 		(ParamInt<px4::params::RC_MAP_OFFB_SW>) _param_rc_map_offb_sw,
@@ -219,7 +260,6 @@ private:
 		(ParamInt<px4::params::RC_MAP_ARM_SW>) _param_rc_map_arm_sw,
 		(ParamInt<px4::params::RC_MAP_TRANS_SW>) _param_rc_map_trans_sw,
 		(ParamInt<px4::params::RC_MAP_GEAR_SW>) _param_rc_map_gear_sw,
-		(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons,
 
 		(ParamInt<px4::params::RC_MAP_AUX1>) _param_rc_map_aux1,
 		(ParamInt<px4::params::RC_MAP_AUX2>) _param_rc_map_aux2,

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -794,9 +794,7 @@ void VehicleIMU::SensorCalibrationSaveAccel()
 			}
 		}
 
-		if (initialised) {
-			_accel_calibration.set_offset({0.1f, 0.2f, 0.3f}); // hack
-
+		if (initialised && ((cal_orig - offset_estimate).longerThan(0.05f) || !_accel_calibration.calibrated())) {
 			if (_accel_calibration.set_offset(offset_estimate)) {
 				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
 					 _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(),
@@ -848,9 +846,7 @@ void VehicleIMU::SensorCalibrationSaveGyro()
 			}
 		}
 
-		if (initialised) {
-			_gyro_calibration.set_offset({0.1f, 0.2f, 0.3f}); // hack
-
+		if (initialised && ((cal_orig - offset_estimate).longerThan(0.01f) || !_gyro_calibration.calibrated())) {
 			if (_gyro_calibration.set_offset(offset_estimate)) {
 				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
 					 _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(),

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -177,7 +177,7 @@ private:
 	InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
 	InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
 
-	static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{1_s};
+	static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s};
 
 	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -234,8 +234,8 @@ void VehicleMagnetometer::UpdateMagCalibration()
 	// This is a reference variance used to calculate the fraction of learned magnetometer bias that will be used to update the stored value.
 	// Larger values cause a larger fraction of the learned biases to be used.
 	static constexpr float magb_vref = 2.5e-7f;
-	// static constexpr float min_var_allowed = magb_vref * 0.01f;
-	// static constexpr float max_var_allowed = magb_vref * 100.f;
+	static constexpr float min_var_allowed = magb_vref * 0.01f;
+	static constexpr float max_var_allowed = magb_vref * 100.f;
 
 	if (_armed) {
 		static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]);
@@ -251,8 +251,8 @@ void VehicleMagnetometer::UpdateMagCalibration()
 				const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
 						   && (estimator_sensor_bias.mag_device_id != 0) &&
 						   estimator_sensor_bias.mag_bias_valid &&
-						   estimator_sensor_bias.mag_bias_stable;/* &&
-						   (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);*/
+						   estimator_sensor_bias.mag_bias_stable &&
+						   (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
 
 				if (valid) {
 					// find corresponding mag calibration
@@ -302,8 +302,6 @@ void VehicleMagnetometer::UpdateMagCalibration()
 						state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain(axis_index)), 0.f);
 					}
 
-					_calibration[mag_index].set_offset({0.1f, 0.2f, 0.3f}); // hack
-
 					if (_calibration[mag_index].set_offset(mag_cal_offset)) {
 
 						PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])",

src/modules/simulator/simulator.h

@@ -56,12 +56,9 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/input_rc.h>
-#include <uORB/topics/esc_status.h>
-#include <uORB/topics/esc_report.h>
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -193,7 +190,6 @@ private:
 	uORB::Publication<differential_pressure_s>	_differential_pressure_pub{ORB_ID(differential_pressure)};
 	uORB::PublicationMulti<optical_flow_s>		_flow_pub{ORB_ID(optical_flow)};
 	uORB::Publication<irlock_report_s>		_irlock_report_pub{ORB_ID(irlock_report)};
-	uORB::Publication<esc_status_s>			_esc_status_pub{ORB_ID(esc_status)};
 	uORB::Publication<vehicle_odometry_s>		_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_odometry_s>		_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 
@@ -236,7 +232,6 @@ private:
 	void send();
 	void send_controls();
 	void send_heartbeat();
-	void send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control);
 	void send_mavlink_message(const mavlink_message_t &aMsg);
 	void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors);
 
@@ -264,14 +259,12 @@ private:
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 
 	// hil map_ref data
 	MapProjection _global_local_proj_ref{};
 	float _global_local_alt0{NAN};
 
 	vehicle_status_s _vehicle_status{};
-	battery_status_s _battery_status{};
 
 	bool _accel_blocked[ACCEL_COUNT_MAX] {};
 	bool _accel_stuck[ACCEL_COUNT_MAX] {};
@@ -300,8 +293,6 @@ private:
 	float _last_baro_pressure{0.0f};
 	float _last_baro_temperature{0.0f};
 
-	int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
-
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	px4::atomic<bool> _has_initialized {false};
 #endif

src/modules/simulator/simulator_mavlink.cpp

@@ -87,12 +87,6 @@ Simulator::Simulator()
 	int32_t sys_ctrl_alloc = 0;
 	param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
 	_use_dynamic_mixing = sys_ctrl_alloc >= 1;
-
-	for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) {
-		char param_name[17];
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1);
-		param_get(param_find(param_name), &_output_functions[i]);
-	}
 }
 
 void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg)
@@ -206,29 +200,6 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
 #endif
 }
 
-void Simulator::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control)
-{
-	esc_status_s esc_status{};
-	esc_status.timestamp = hrt_absolute_time();
-	esc_status.esc_count = math::min(actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, esc_status_s::CONNECTED_ESC_MAX);
-
-	const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-	esc_status.esc_armed_flags = armed ? 255 : 0;  // ugly
-
-	for (int i = 0; i < esc_status.esc_count; i++) {
-		esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_out_sim...
-		esc_status.esc[i].timestamp = esc_status.timestamp;
-		esc_status.esc[i].esc_errorcount = 0; // TODO
-		esc_status.esc[i].esc_voltage = _battery_status.voltage_v;
-		esc_status.esc[i].esc_current = armed ? 1.0 + math::abs_t(hil_act_control.controls[i]) * 15.0 :
-						0.0f; // TODO: magic number
-		esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000;  // TODO: magic number
-		esc_status.esc[i].esc_temperature = 20.0 + math::abs_t(hil_act_control.controls[i]) * 40.0;
-	}
-
-	_esc_status_pub.publish(esc_status);
-}
-
 void Simulator::send_controls()
 {
 	orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
@@ -243,8 +214,6 @@ void Simulator::send_controls()
 		PX4_DEBUG("sending controls t=%ld (%ld)", _actuator_outputs.timestamp, hil_act_control.time_usec);
 
 		send_mavlink_message(message);
-
-		send_esc_telemetry(hil_act_control);
 	}
 }
 
@@ -787,7 +756,6 @@ void Simulator::send()
 			parameters_update(false);
 			check_failure_injections();
 			_vehicle_status_sub.update(&_vehicle_status);
-			_battery_status_sub.update(&_battery_status);
 
 			// Wait for other modules, such as logger or ekf2
 			px4_lockstep_wait_for_components();

src/systemcmds/pwm/pwm.cpp

@@ -100,12 +100,12 @@ The parameters `-p` and `-r` can be set to a parameter instead of specifying an
 Note that in OneShot mode, the PWM range [1000, 2000] is automatically mapped to [125, 250].
 
 ### Examples
-
 Set the PWM rate for all channels to 400 Hz:
 $ pwm rate -a -r 400
 
-Arm and set the outputs of channels 1 and 3 to a PWM value to 1200 us:
-$ pwm min -c 13 -p 1200
+Test the outputs of eg. channels 1 and 3, and set the PWM value to 1200 us:
+$ pwm arm
+$ pwm test -c 13 -p 1200
 
 )DESCR_STR");
 

test/mavsdk_tests/CMakeLists.txt

@@ -16,9 +16,6 @@ if(MAVSDK_FOUND)
     add_executable(mavsdk_tests
         test_main.cpp
         autopilot_tester.cpp
-        autopilot_tester_failure.cpp
-        test_multicopter_control_allocation.cpp
-        test_multicopter_failure_injection.cpp
         test_multicopter_failsafe.cpp
         test_multicopter_mission.cpp
         test_multicopter_offboard.cpp

test/mavsdk_tests/autopilot_tester.cpp

@@ -131,14 +131,6 @@ void AutopilotTester::set_takeoff_altitude(const float altitude_m)
 	CHECK(result.second == Approx(altitude_m));
 }
 
-void AutopilotTester::set_rtl_altitude(const float altitude_m)
-{
-	CHECK(Action::Result::Success == _action->set_return_to_launch_altitude(altitude_m));
-	const auto result = _action->get_return_to_launch_altitude();
-	CHECK(result.first == Action::Result::Success);
-	CHECK(result.second == Approx(altitude_m));
-}
-
 void AutopilotTester::set_height_source(AutopilotTester::HeightSource height_source)
 {
 	switch (height_source) {
@@ -205,22 +197,7 @@ void AutopilotTester::wait_until_disarmed(std::chrono::seconds timeout_duration)
 
 void AutopilotTester::wait_until_hovering()
 {
-	wait_for_landed_state(Telemetry::LandedState::InAir, std::chrono::seconds(45));
-}
-
-void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout)
-{
-	auto prom = std::promise<void> {};
-	auto fut = prom.get_future();
-
-	_telemetry->subscribe_position([&prom, rel_altitude_m, this](Telemetry::Position new_position) {
-		if (fabs(rel_altitude_m - new_position.relative_altitude_m) <= 0.5) {
-			_telemetry->subscribe_position(nullptr);
-			prom.set_value();
-		}
-	});
-
-	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
+	wait_for_landed_state(Telemetry::LandedState::InAir, std::chrono::seconds(30));
 }
 
 void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
@@ -265,7 +242,7 @@ void AutopilotTester::execute_mission()
 
 	// TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.
 
-	wait_for_mission_finished(std::chrono::seconds(90));
+	wait_for_mission_finished(std::chrono::seconds(60));
 }
 
 void AutopilotTester::execute_mission_and_lose_gps()
@@ -390,11 +367,6 @@ void AutopilotTester::execute_rtl()
 	REQUIRE(Action::Result::Success == _action->return_to_launch());
 }
 
-void AutopilotTester::execute_land()
-{
-	REQUIRE(Action::Result::Success == _action->land());
-}
-
 void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m,
 				    std::chrono::seconds timeout_duration)
 {
@@ -489,22 +461,6 @@ void AutopilotTester::fly_forward_in_altctl()
 	}
 }
 
-void AutopilotTester::start_checking_altitude(const float max_deviation_m)
-{
-	std::array<float, 3> initial_position = get_current_position_ned();
-	float target_altitude = initial_position[2];
-
-	_telemetry->subscribe_position([target_altitude, max_deviation_m, this](Telemetry::Position new_position) {
-		const float current_deviation = fabs((-target_altitude) - new_position.relative_altitude_m);
-		CHECK(current_deviation <= max_deviation_m);
-	});
-}
-
-void AutopilotTester::stop_checking_altitude()
-{
-	_telemetry->subscribe_position(nullptr);
-}
-
 void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 {
 	auto mission = _mission->download_mission();
@@ -534,12 +490,6 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 	});
 }
 
-std::array<float, 3> AutopilotTester::get_current_position_ned()
-{
-	mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
-	std::array<float, 3> position_ned{position_velocity_ned.position.north_m, position_velocity_ned.position.east_m, position_velocity_ned.position.down_m};
-	return position_ned;
-}
 
 void AutopilotTester::offboard_land()
 {
@@ -690,27 +640,6 @@ void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state,
 	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
 }
 
-void AutopilotTester::wait_until_speed_lower_than(float speed, std::chrono::seconds timeout)
-{
-	auto prom = std::promise<void> {};
-	auto fut = prom.get_future();
-
-	_telemetry->subscribe_position_velocity_ned([&prom, speed, this](Telemetry::PositionVelocityNed position_velocity_ned) {
-		std::array<float, 3> current_velocity;
-		current_velocity[0] = position_velocity_ned.velocity.north_m_s;
-		current_velocity[1] = position_velocity_ned.velocity.east_m_s;
-		current_velocity[2] = position_velocity_ned.velocity.down_m_s;
-		const float current_speed = norm(current_velocity);
-
-		if (current_speed <= speed) {
-			_telemetry->subscribe_position_velocity_ned(nullptr);
-			prom.set_value();
-		}
-	});
-
-	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
-}
-
 void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout)
 {
 	auto prom = std::promise<void> {};

test/mavsdk_tests/autopilot_tester.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 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
@@ -101,7 +101,6 @@ public:
 	void check_home_within(float acceptance_radius_m);
 	void check_home_not_within(float min_distance_m);
 	void set_takeoff_altitude(const float altitude_m);
-	void set_rtl_altitude(const float altitude_m);
 	void set_height_source(HeightSource height_source);
 	void set_rc_loss_exception(RcLossException mask);
 	void arm();
@@ -110,9 +109,7 @@ public:
 	void transition_to_fixedwing();
 	void transition_to_multicopter();
 	void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(60));
-	void wait_until_hovering(); // TODO: name suggests, that function waits for drone velocity to be zero and not just drone in the air
-	void wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout);
-	void wait_until_speed_lower_than(float speed, std::chrono::seconds timeout);
+	void wait_until_hovering();
 	void prepare_square_mission(MissionOptions mission_options);
 	void prepare_straight_mission(MissionOptions mission_options);
 	void execute_mission();
@@ -124,7 +121,6 @@ public:
 	void load_qgc_mission_raw_and_move_here(const std::string &plan_file);
 	void execute_mission_raw();
 	void execute_rtl();
-	void execute_land();
 	void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f,
 			   std::chrono::seconds timeout_duration = std::chrono::seconds(60));
 	void offboard_land();
@@ -133,16 +129,7 @@ public:
 	void request_ground_truth();
 	void check_mission_item_speed_above(int item_index, float min_speed_m_s);
 	void check_tracks_mission(float corridor_radius_m = 1.5f);
-	void start_checking_altitude(const float max_deviation_m);
-	void stop_checking_altitude();
 
-	// Blocking call to get the drone's current position in NED frame
-	std::array<float, 3> get_current_position_ned();
-
-protected:
-	mavsdk::Param *getParams() const { return _param.get();}
-	mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
-	std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
 
 private:
 	mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();

test/mavsdk_tests/autopilot_tester_failure.cpp

@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
-
-#include "autopilot_tester_failure.h"
-
-#include "math_helpers.h"
-#include <iostream>
-#include <future>
-#include <thread>
-#include <unistd.h>
-
-
-void AutopilotTesterFailure::connect(const std::string uri)
-{
-	AutopilotTester::connect(uri);
-
-	auto system = get_system();
-	_failure.reset(new Failure(system));
-}
-
-void AutopilotTesterFailure::set_param_sys_failure_en(bool value)
-{
-	CHECK(getParams()->set_param_int("SYS_FAILURE_EN", value) == Param::Result::Success);
-}
-
-void AutopilotTesterFailure::set_param_fd_act_en(bool value)
-{
-	CHECK(getParams()->set_param_int("FD_ACT_EN", value) == Param::Result::Success);
-}
-
-void AutopilotTesterFailure::set_param_mc_airmode(int value)
-{
-	CHECK(getParams()->set_param_int("MC_AIRMODE", value) == Param::Result::Success);
-}
-
-void AutopilotTesterFailure::set_param_ca_failure_mode(int value)
-{
-	CHECK(getParams()->set_param_int("CA_FAILURE_MODE", value) == Param::Result::Success);
-}
-
-void AutopilotTesterFailure::set_param_com_act_fail_act(int value)
-{
-	CHECK(getParams()->set_param_int("COM_ACT_FAIL_ACT", value) == Param::Result::Success);
-}
-
-void AutopilotTesterFailure::inject_failure(mavsdk::Failure::FailureUnit failure_unit,
-		mavsdk::Failure::FailureType failure_type, int instance, mavsdk::Failure::Result expected_result)
-{
-	CHECK(_failure->inject(failure_unit, failure_type, instance) == expected_result);
-}
-
-void AutopilotTesterFailure::enable_actuator_output_status()
-{
-	CHECK(getTelemetry()->set_rate_actuator_output_status(20.f) == Telemetry::Result::Success);
-}
-
-void AutopilotTesterFailure::ensure_motor_stopped(unsigned index, unsigned num_motors)
-{
-	const Telemetry::ActuatorOutputStatus &status = getTelemetry()->actuator_output_status();
-	CHECK(status.active >= num_motors);
-
-	for (unsigned i = 0; i < num_motors; ++i) {
-		if (i == index) {
-			CHECK(status.actuator[i] <= 901.f);
-
-		} else { // ensure all others are still running
-			CHECK(status.actuator[i] >= 999.f);
-		}
-	}
-}

test/mavsdk_tests/configs/sitl.json

@@ -21,12 +21,6 @@
             "vehicle": "tailsitter",
             "test_filter": "[vtol]",
             "timeout_min": 10
-        },
-        {
-            "model": "typhoon_h480_ctrlalloc",
-            "vehicle": "typhoon_h480_ctrlalloc",
-            "test_filter": "[controlallocation]",
-            "timeout_min": 10
         }
     ]
 }

test/mavsdk_tests/test_multicopter_control_allocation.cpp

@@ -1,304 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
-
-#include <thread>
-#include <chrono>
-#include <math.h>
-
-#include "autopilot_tester_failure.h"
-
-TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]")
-{
-	const float flight_altitude = 10.0f;
-	const float altitude_tolerance = 4.0f;
-	const float hover_speed_tolerance = 1.0f;
-
-	AutopilotTester::MissionOptions mission_options;
-	mission_options.rtl_at_end = false;
-	mission_options.relative_altitude_m = flight_altitude;
-
-	AutopilotTesterFailure tester;
-	tester.connect(connection_url);
-	tester.wait_until_ready();
-
-	// Configuration
-	tester.set_param_sys_failure_en(true);  // Enable failure injection
-	tester.set_param_fd_act_en(true);	// Enable motor failure detection
-	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
-	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
-	tester.prepare_square_mission(mission_options);
-	tester.set_takeoff_altitude(flight_altitude);
-	tester.set_rtl_altitude(flight_altitude);
-	tester.check_tracks_mission(5.f);
-	tester.store_home();
-	tester.enable_actuator_output_status();
-	std::this_thread::sleep_for(std::chrono::seconds(
-					    1));  // This is necessary for the takeoff altitude to be applied properly
-
-	// Takeoff
-	tester.arm();
-	tester.takeoff();
-	tester.wait_until_hovering();
-	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
-	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
-
-	// Motor failure mid-air
-	tester.start_checking_altitude(altitude_tolerance);
-	const int motor_instance = 1;
-	const unsigned num_motors = 6; // TODO: get from model
-	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance,
-			      mavsdk::Failure::Result::Success);
-	std::this_thread::sleep_for(std::chrono::seconds(1));
-	tester.ensure_motor_stopped(motor_instance - 1, num_motors);
-
-	tester.execute_mission();
-	tester.stop_checking_altitude();
-	tester.ensure_motor_stopped(motor_instance - 1, num_motors); // just to be sure
-
-	// RTL
-	tester.execute_rtl();
-	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
-	tester.wait_until_disarmed(until_disarmed_timeout);
-	tester.check_home_within(5.f);
-}
-
-TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]")
-{
-	const float flight_altitude = 10.0f;
-	const float altitude_tolerance = 4.0f;
-	const float hover_speed_tolerance = 1.0f;
-
-	AutopilotTesterFailure tester;
-	tester.connect(connection_url);
-	tester.wait_until_ready();
-	tester.set_param_sys_failure_en(true);  // Enable failure injection
-	tester.set_param_fd_act_en(true);	// Enable motor failure detection
-	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
-	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
-
-	AutopilotTester::MissionOptions mission_options;
-	mission_options.rtl_at_end = false;
-	mission_options.relative_altitude_m = flight_altitude;
-
-	tester.prepare_square_mission(mission_options);
-	tester.set_takeoff_altitude(flight_altitude);
-	tester.set_rtl_altitude(flight_altitude);
-	tester.check_tracks_mission(5.f);
-	tester.store_home();
-	std::this_thread::sleep_for(std::chrono::seconds(
-					    1));  // This is necessary for the takeoff altitude to be applied properly
-
-	tester.arm();
-	tester.takeoff();
-	tester.wait_until_hovering();
-	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
-	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
-
-	// Remove two motors opposite of one another on the hexa airframe
-	const int first_motor_instance = 1;
-	const int second_motor_instance = 2;
-	tester.start_checking_altitude(altitude_tolerance);
-	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
-			      first_motor_instance,
-			      mavsdk::Failure::Result::Success);
-	std::this_thread::sleep_for(std::chrono::seconds(1));
-	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
-			      second_motor_instance,
-			      mavsdk::Failure::Result::Success);
-	std::this_thread::sleep_for(std::chrono::seconds(1));
-
-	tester.execute_mission();
-	tester.stop_checking_altitude();
-
-	// RTL with two motors out won't work because navigator will wait forever until
-	// the yaw setpoint is reached during RTL, and it won't land.
-	tester.land();
-	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
-	tester.wait_until_disarmed(until_disarmed_timeout);
-}
-
-TEST_CASE("Control Allocation - Remove and restore every motor once", "[controlallocation]")
-{
-	const float flight_altitude = 10.0f;
-	const float altitude_tolerance = 4.0f;
-	const float hover_speed_tolerance = 1.0f;
-
-	AutopilotTesterFailure tester;
-	tester.connect(connection_url);
-	tester.wait_until_ready();
-	tester.set_param_sys_failure_en(true);  // Enable failure injection
-	tester.set_param_fd_act_en(true);	// Enable motor failure detection
-	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
-	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
-
-	AutopilotTester::MissionOptions mission_options;
-	mission_options.rtl_at_end = false;
-	mission_options.relative_altitude_m = flight_altitude;
-
-	tester.prepare_square_mission(mission_options);
-	tester.set_takeoff_altitude(flight_altitude);
-	tester.set_rtl_altitude(flight_altitude);
-	tester.check_tracks_mission(5.f);
-	tester.store_home();
-	std::this_thread::sleep_for(std::chrono::seconds(
-					    1));  // This is necessary for the takeoff altitude to be applied properly
-
-	tester.arm();
-	tester.takeoff();
-	tester.wait_until_hovering();
-	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
-	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
-
-	tester.start_checking_altitude(altitude_tolerance);
-
-	for (int m = 1; m <= 6; m++) {
-		tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, m,
-				      mavsdk::Failure::Result::Success);
-		std::this_thread::sleep_for(std::chrono::seconds(1));
-		tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Ok, m,
-				      mavsdk::Failure::Result::Success);
-		std::this_thread::sleep_for(std::chrono::seconds(1));
-	}
-
-	tester.execute_mission();
-	tester.stop_checking_altitude();
-
-	tester.execute_rtl();
-	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
-	tester.wait_until_disarmed(until_disarmed_timeout);
-	tester.check_home_within(5.f);
-}
-
-TEST_CASE("Control Allocation - Return home on motor failure", "[controlallocation]")
-{
-	const float flight_altitude = 10.0f;
-	const float hover_speed_tolerance = 1.0f;
-
-	AutopilotTesterFailure tester;
-	tester.connect(connection_url);
-	tester.wait_until_ready();
-
-	// Configuration
-	tester.set_param_sys_failure_en(true);  // Enable failure injection
-	tester.set_param_fd_act_en(true);	// Enable motor failure detection
-	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
-	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
-	tester.set_param_com_act_fail_act(3);	// RTL on motor failure
-	tester.set_takeoff_altitude(flight_altitude);
-	tester.store_home();
-	std::this_thread::sleep_for(std::chrono::seconds(
-					    1));  // This is necessary for the takeoff altitude to be applied properly
-
-	// Takeoff
-	tester.arm();
-	tester.takeoff();
-	tester.wait_until_hovering();
-	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
-	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
-
-	// TODO: Minor improvement, fly forward for a little bit before triggering motor failure to distinguish "RTL" and "Land only"
-
-	// Motor failure mid-air
-	const int motor_instance = 1;
-	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance,
-			      mavsdk::Failure::Result::Success);
-	std::this_thread::sleep_for(std::chrono::seconds(1));
-
-	// Wait for RTL to trigger automatically
-	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
-	tester.wait_until_disarmed(until_disarmed_timeout);
-	tester.check_home_within(5.f);
-}
-
-TEST_CASE("Control Allocation - Terminate on motor failure", "[controlallocation]")
-{
-	const float flight_altitude = 100.0f;
-	const float hover_speed_tolerance = 1.0f;
-
-	AutopilotTesterFailure tester;
-	tester.connect(connection_url);
-	tester.wait_until_ready();
-
-	// Configuration
-	tester.set_param_sys_failure_en(true);  // Enable failure injection
-	tester.set_param_fd_act_en(true);	// Enable motor failure detection
-	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
-	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
-	tester.set_param_com_act_fail_act(4);	// Terminate on motor failure
-	tester.set_takeoff_altitude(flight_altitude);
-	std::this_thread::sleep_for(std::chrono::seconds(
-					    1));  // This is necessary for the takeoff altitude to be applied properly
-
-	// Takeoff
-	tester.arm();
-	tester.takeoff();
-	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(60));
-	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(60));
-
-	// Motor failure mid-air
-	const int motor_instance = 1;
-	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance,
-			      mavsdk::Failure::Result::Success);
-	std::this_thread::sleep_for(std::chrono::seconds(1));
-
-	// Wait for disarm with a low enough timeout such that it's necessary for the
-	// drone to freefall (terminate) in order to disarm quickly enough:
-	// h = g/2 * t^2 -> solve for t
-	const int seconds_to_touchdown = 2 + sqrt(flight_altitude * 2 / 10.0);
-	std::cout << "seconds_to_touchdown: " << seconds_to_touchdown << std::endl;
-	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(seconds_to_touchdown);
-	tester.wait_until_disarmed(until_disarmed_timeout);
-}
-
-#if 0
-// This is for checking that the SITL test is actually capable of detecting the drone crash
-// when not reallocating the control allocation on a motor failure
-TEST_CASE("Control Allocation - Remove two motors and expect crash", "[controlallocation]")
-{
-	// TODO
-}
-#endif
-
-#if 0
-TEST_CASE("Control Allocation with multiple sequential motor failures", "[controlallocation]")
-{
-	// TODO
-}
-#endif
-
-#if 0
-TEST_CASE("Control Allocation with multiple simultaneous motor failures", "[controlallocation]")
-{
-	// TODO
-}
-#endif

test/mavsdk_tests/test_multicopter_failure_injection.cpp

@@ -1,61 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
-
-#include <thread>
-#include <chrono>
-
-#include "autopilot_tester_failure.h"
-
-TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopter]")
-{
-	AutopilotTesterFailure tester;
-	tester.connect(connection_url);
-	tester.wait_until_ready();
-	tester.arm();
-	tester.takeoff();
-	tester.wait_until_hovering();
-	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, 1,
-			      mavsdk::Failure::Result::Disabled);
-	tester.execute_rtl();
-	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
-	tester.wait_until_disarmed(until_disarmed_timeout);
-}
-
-TEST_CASE("Failure Injection - Reject before arming", "[multicopter]")
-{
-	AutopilotTesterFailure tester;
-	tester.connect(connection_url);
-	tester.wait_until_ready();
-	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, 1,
-			      mavsdk::Failure::Result::Disabled);
-}