PositionControl: log individual portions of PID velocity control

trajectory_setpoint: correct comment typo "kinematically"
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
2026-06-08 07:20:06 +08:00 · 2022-05-12 17:21:20 +02:00 · 2022-05-12 17:19:48 +02:00 · 2022-05-12 17:19:48 +02:00 · 2022-05-12 17:19:48 +02:00 · 2022-05-12 17:19:48 +02:00
112 changed files with 1681 additions and 781 deletions
@@ -7,6 +7,11 @@

 . ${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

@@ -1,6 +1,4 @@

-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
@@ -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_1,         /* SPLLDIV2 */
+			.div2        = SCG_ASYNC_CLOCK_DIV_BY_2,         /* SPLLDIV2 */
 			.prediv      = 1,                                /* PREDIV */
 			.mult        = 40,                               /* MULT */
 			.src         = 0,                                /* SOURCE */
@@ -101,7 +101,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SIRC_DIV2,
+		.clksrc  = CLK_SRC_SPLL_DIV2,
 	},
 	{
 		.clkname = LPSPI0_CLK,
@@ -110,7 +110,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SIRC_DIV2,
+		.clksrc  = CLK_SRC_SPLL_DIV2,
 	},
 	{
 		.clkname = LPUART0_CLK,
@@ -119,7 +119,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SIRC_DIV2,
+		.clksrc  = CLK_SRC_SPLL_DIV2,
 	},
 	{
 		.clkname = LPUART1_CLK,
@@ -128,7 +128,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SIRC_DIV2,
+		.clksrc  = CLK_SRC_SPLL_DIV2,
 	},
 	{
 		.clkname    = RTC0_CLK,
@@ -164,6 +164,7 @@ set(msg_files
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg
+	trajectory_setpoint.msg
 	trajectory_waypoint.msg
 	transponder_report.msg
 	tune_control.msg
@@ -4,6 +4,8 @@ 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),
@@ -19,3 +19,5 @@ 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
@@ -8,6 +8,8 @@ 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)
@@ -8,5 +8,7 @@ bool fd_ext
 bool fd_arm_escs
 bool fd_battery
 bool fd_imbalanced_prop
+bool fd_motor

 float32 imbalanced_prop_metric      # Metric of the imbalanced propeller check (low-passed)
+uint16 motor_failure_mask           # Bit-mask with motor indices, indicating critical motor failures
@@ -59,8 +59,7 @@ rtps:
    send:    true
  - msg:     vehicle_local_position_setpoint
    receive: true
-  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
-    base:    vehicle_local_position_setpoint
+  - msg:     trajectory_setpoint
    receive: true
  - msg:     vehicle_odometry
    send:    true
@@ -0,0 +1,15 @@
+# Trajectory setpoint in NED frame
+# Input to PID position controller.
+# Needs to be kinematically consistent and feasible for smooth flight.
+# setting a value to NaN means the state should not be controlled
+
+uint64 timestamp # time since system start (microseconds)
+
+# NED local world frame
+float32[3] position # in meters
+float32[3] velocity # in meters/second
+float32[3] acceleration # in meters/second^2
+float32[3] jerk # in meters/second^3 (for logging only)
+
+float32 yaw # euler angle of desired attitude in radians -PI..+PI
+float32 yawspeed # angular velocity around NED frame z-axis in radians/second
@@ -1,18 +1,23 @@
 # Local position setpoint in NED frame
-# setting something to NaN means the state should not be controlled
+# Telemetry of PID position controller to monitor tracking.
+# NaN means the state was not controlled

 uint64 timestamp	# time since system start (microseconds)

 float32 x		# in meters NED
 float32 y		# in meters NED
 float32 z		# in meters NED
-float32 yaw		# in radians NED -PI..+PI
-float32 yawspeed	# in radians/sec
+
 float32 vx		# in meters/sec
 float32 vy		# in meters/sec
 float32 vz		# in meters/sec
+
 float32[3] acceleration # in meters/sec^2
-float32[3] jerk # in meters/sec^3
 float32[3] thrust	# normalized thrust vector in NED

-# TOPICS vehicle_local_position_setpoint trajectory_setpoint
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec
+
+float32[3] velocity_proportional_portion
+float32[3] velocity_integral_potion
+float32[3] velocity_derivative_portion
@@ -11,14 +11,15 @@ uint8 ARMING_STATE_IN_AIR_RESTORE = 5
 uint8 ARMING_STATE_MAX = 6

 # FailureDetector status
-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)
+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)

 # HIL
 uint8 HIL_STATE_OFF = 0
@@ -31,7 +32,7 @@ uint8 NAVIGATION_STATE_POSCTL = 2		# Position control mode
 uint8 NAVIGATION_STATE_AUTO_MISSION = 3		# Auto mission mode
 uint8 NAVIGATION_STATE_AUTO_LOITER = 4		# Auto loiter mode
 uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
-uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 	# Auto land on engine failure
+uint8 NAVIGATION_STATE_UNUSED3 = 8 	        # Free slot
 uint8 NAVIGATION_STATE_UNUSED = 9	        # Free slot
 uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
 uint8 NAVIGATION_STATE_UNUSED1 = 11		# Free slot
@@ -85,11 +86,10 @@ uint8 data_link_lost_counter			# counts unique data link lost events

 bool high_latency_data_link_lost 			# Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost

-bool engine_failure				# Set to true if an engine failure is detected
 bool mission_failure				# Set to true if mission could not continue/finish
 bool geofence_violated

-uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
+uint16 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]

 # see SYS_STATUS mavlink message for the following
 # lower 32 bits are for the base flags, higher 32 bits are or the extended flags
@@ -28,7 +28,6 @@ bool flight_terminated

 bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
-bool circuit_breaker_engaged_enginefailure_check
 bool circuit_breaker_flight_termination_disabled
 bool circuit_breaker_engaged_usb_check
 bool circuit_breaker_engaged_posfailure_check	# set to true when the position valid checks have been disabled
@@ -52,18 +52,18 @@
 #endif // NuttX


-O1HeapInstance *uavcan_allocator{nullptr};
+O1HeapInstance *cyphal_allocator{nullptr};

-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); }
+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); }


 CanardHandle::CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes)
 {
-	_uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
-	uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr);
+	_cyphal_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
+	cyphal_allocator = o1heapInit(_cyphal_heap, HeapSize, nullptr, nullptr);

-	if (uavcan_allocator == nullptr) {
+	if (cyphal_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 *>(_uavcan_heap);
-	_uavcan_heap = nullptr;
+	delete static_cast<uint8_t *>(_cyphal_heap);
+	_cyphal_heap = nullptr;

 }

@@ -200,6 +200,11 @@ 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;
@@ -34,6 +34,7 @@
 #pragma once

 #include <canard.h>
+#include "o1heap/o1heap.h"
 #include "CanardInterface.hpp"

 class CanardHandle
@@ -68,6 +69,7 @@ public:
 	int8_t RxUnsubscribe(const CanardTransferKind transfer_kind,
 			     const CanardPortID       port_id);
 	CanardTreeNode *getRxSubscriptions(CanardTransferKind kind);
+	O1HeapDiagnostics getO1HeapDiagnostics();

 	int32_t mtu();
 	CanardNodeID node_id();
@@ -80,6 +82,6 @@ private:

 	CanardTxQueue _queue;

-	void *_uavcan_heap{nullptr};
+	void *_cyphal_heap{nullptr};

 };
@@ -245,12 +245,12 @@ void CyphalNode::print_info()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);

-	/*O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
+	O1HeapDiagnostics heap_diagnostics = _canard_handle.getO1HeapDiagnostics();

 	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();

@@ -247,7 +247,10 @@ void DShot::update_telemetry_num_motors()

 	if (_mixing_output.useDynamicMixing()) {
 		for (unsigned i = 0; i < _num_outputs; ++i) {
-			motor_count += _mixing_output.isFunctionSet(i);
+			if (_mixing_output.isFunctionSet(i)) {
+				_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
+				++motor_count;
+			}
 		}

 	} else {
@@ -281,24 +284,26 @@ void DShot::init_telemetry(const char *device)
 	update_telemetry_num_motors();
 }

-void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data)
+void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
 {
 	// fill in new motor data
 	esc_status_s &esc_status = _telemetry->esc_status_pub.get();

-	if (motor_index < esc_status_s::CONNECTED_ESC_MAX) {
-		esc_status.esc_online_flags |= 1 << motor_index;
+	if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) {
+		esc_status.esc_online_flags |= 1 << telemetry_index;

-		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);
+		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);
 		// TODO: accumulate consumption and use for battery estimation
 	}

 	// publish when motor index wraps (which is robust against motor timeouts)
-	if (motor_index <= _telemetry->last_motor_index) {
+	if (telemetry_index <= _telemetry->last_telemetry_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();
@@ -314,7 +319,7 @@ void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetr
 		esc_status.esc_online_flags = 0;
 	}

-	_telemetry->last_motor_index = motor_index;
+	_telemetry->last_telemetry_index = telemetry_index;
 }

 int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)
@@ -493,8 +498,6 @@ 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();
@@ -503,6 +506,8 @@ void DShot::Run()
 		return;
 	}

+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);

 	_mixing_output.update();
@@ -127,14 +127,15 @@ private:
 	struct Telemetry {
 		DShotTelemetry handler{};
 		uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
-		int last_motor_index{-1};
+		int last_telemetry_index{-1};
+		uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
 	};

 	void enable_dshot_outputs(const bool enabled);

 	void init_telemetry(const char *device);

-	void handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data);
+	void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);

 	int request_esc_info();

@@ -126,8 +126,6 @@ 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();
@@ -136,6 +134,8 @@ void LinuxPWMOut::Run()
 		return;
 	}

+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);

@@ -408,10 +408,6 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state)
 		flight_mode = "AUTO";
 		break;

-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		flight_mode = "FAILURE";
-		break;
-
 	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		flight_mode = "ACRO";
 		break;
@@ -376,8 +376,6 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned

 void PCA9685Wrapper::Run()
 {
-	SmartLock lock_guard(_lock);
-
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -391,6 +389,8 @@ void PCA9685Wrapper::Run()
 		return;
 	}

+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);

 	switch (_state) {
@@ -439,8 +439,6 @@ 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();
@@ -449,6 +447,8 @@ void PWMOut::Run()
 		return;
 	}

+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);

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

+	SmartLock lock_guard(_lock);
+
 	_mixing_output.update();

 	// check for parameter updates
@@ -525,8 +525,6 @@ void PX4IO::updateFailsafe()

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

+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);

@@ -48,12 +48,6 @@ 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;
@@ -116,6 +110,8 @@ RCInput::init()
 	px4_arch_unconfiggpio(GPIO_PPM_IN);
 #endif // GPIO_PPM_IN

+	rc_io_invert(false);
+
 	return 0;
 }

@@ -271,8 +267,6 @@ 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)
@@ -417,8 +411,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 300 msec, then switch protocol
-		constexpr hrt_abstime rc_scan_max = 300_ms;
+		// Scan for 500 msec, then switch protocol
+		constexpr hrt_abstime rc_scan_max = 500_ms;

 		unsigned frame_drops = 0;

@@ -434,6 +428,8 @@ void RCInput::Run()
 			_bytes_rx += newBytes;
 		}

+		const bool rc_scan_locked = _rc_scan_locked;
+
 		switch (_rc_scan_state) {
 		case RC_SCAN_NONE:
 			// do nothing
@@ -754,15 +750,18 @@ void RCInput::Run()
 			_rc_scan_locked = false;
 		}

-		if (_report_lock && _rc_scan_locked) {
-			_report_lock = false;
+		if (!rc_scan_locked && _rc_scan_locked) {
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
+		}

-			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();
-			}
+		// 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();
 		}
 	}
 }
@@ -131,7 +131,6 @@ private:

 	bool _initialized{false};
 	bool _rc_scan_locked{false};
-	bool _report_lock{true};

 	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz

@@ -162,10 +162,6 @@ bool CRSFTelemetry::send_flight_mode()
 		flight_mode = "Auto";
 		break;

-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		flight_mode = "Failure";
-		break;
-
 	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		flight_mode = "Acro";
 		break;
@@ -290,6 +290,8 @@ 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)
@@ -327,8 +329,6 @@ 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,6 +337,8 @@ void TAP_ESC::Run()
 		return;
 	}

+	SmartLock lock_guard(_lock);
+
 	// push backup schedule
 	ScheduleDelayed(20_ms);

@@ -48,6 +48,7 @@
 #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>
@@ -114,6 +115,7 @@ 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;
@@ -78,6 +78,8 @@ 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.
@@ -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.;
+	battery_status[instance].current_average_a = msg.current;

 	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.;
+	battery_status[instance].capacity = msg.full_charge_capacity_wh;
 	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 = msg.;
+	battery_status[instance].time_remaining_s = NAN;
 	// battery_status[instance].average_time_to_empty = msg.;
 	battery_status[instance].serial_number = msg.model_instance_id;
 	battery_status[instance].id = msg.getSrcNodeID().get();
@@ -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] {};
+	bool battery_aux_support[battery_status_s::MAX_INSTANCES] {false};
 };
@@ -1003,6 +1003,10 @@ 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 {
@@ -56,7 +56,6 @@
 #define CBRK_IO_SAFETY_KEY	22027
 #define CBRK_AIRSPD_CHK_KEY	162128
 #define CBRK_FLIGHTTERM_KEY	121212
-#define CBRK_ENGINEFAIL_KEY	284953
 #define CBRK_USB_CHK_KEY	197848
 #define CBRK_VELPOSERR_KEY	201607
 #define CBRK_VTOLARMING_KEY	159753
@@ -115,22 +115,6 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
 */
 PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);

-/**
- * Circuit breaker for engine failure detection
- *
- * Setting this parameter to 284953 will disable the engine failure detection.
- * If the aircraft is in engine failure mode the engine failure flag will be
- * set to healthy
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @reboot_required true
- * @min 0
- * @max 284953
- * @category Developer
- * @group Circuit Breaker
- */
-PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
-
 /**
 * Circuit breaker for disabling buzzer
 *
@@ -116,6 +116,8 @@ 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); }

@@ -721,7 +721,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 			return TRANSITION_DENIED;
 		}

-		if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)
+		if ((_geofence_result.geofence_action == 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,6 +803,12 @@ 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;

@@ -825,6 +831,13 @@ 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()
@@ -2035,15 +2048,84 @@ 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();
@@ -2070,29 +2152,6 @@ 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
@@ -2100,7 +2159,6 @@ 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
@@ -2114,94 +2172,17 @@ Commander::run()
 		/* update parameters */
 		const bool params_updated = _parameter_update_sub.updated();

-		if (params_updated || param_init_forced) {
+		if (params_updated) {
 			// clear update
 			parameter_update_s update;
 			_parameter_update_sub.copy(&update);

-			// update parameters from storage
-			updateParams();
-
 			/* update parameters */
 			if (!_armed.armed) {
-				_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();
+				updateParameters();

 				_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 */
@@ -2525,16 +2506,16 @@ Commander::run()
 		}

 		/* start geofence result check */
-		_geofence_result_sub.update(&_geofence_result);
-		_status.geofence_violated = _geofence_result.geofence_violated;
+		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;
+		}

 		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_action_enabled
+		    && (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)
 		    && !in_low_battery_failsafe_delay) {

 			// check for geofence violation transition
@@ -2681,10 +2662,9 @@ 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++;
@@ -2754,49 +2734,6 @@ Commander::run()

 		avoidance_check();

-		// engine failure detection
-		// TODO: move out of commander
-		if (_actuator_controls_sub.updated()) {
-			/* Check engine failure
-			 * only for fixed wing for now
-			 */
-			if (!_status_flags.circuit_breaker_engaged_enginefailure_check &&
-			    _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol && _armed.armed) {
-
-				actuator_controls_s actuator_controls{};
-				_actuator_controls_sub.copy(&actuator_controls);
-
-				const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
-				const float current2throttle = _battery_current / throttle;
-
-				if (((throttle > _param_ef_throttle_thres.get()) && (current2throttle < _param_ef_current2throttle_thres.get()))
-				    || _status.engine_failure) {
-
-					const float elapsed = hrt_elapsed_time(&_timestamp_engine_healthy) / 1e6f;
-
-					/* potential failure, measure time */
-					if ((_timestamp_engine_healthy > 0) && (elapsed > _param_ef_time_thres.get())
-					    && !_status.engine_failure) {
-
-						_status.engine_failure = true;
-						_status_changed = true;
-
-						PX4_ERR("Engine Failure");
-						set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, _status);
-					}
-				}
-
-			} else {
-				/* no failure reset flag */
-				_timestamp_engine_healthy = hrt_absolute_time();
-
-				if (_status.engine_failure) {
-					_status.engine_failure = false;
-					_status_changed = true;
-				}
-			}
-		}
-
 		/* check if we are disarmed and there is a better mode to wait in */
 		if (!_armed.armed) {
 			/* if there is no radio control but GPS lock the user might want to fly using
@@ -2842,6 +2779,8 @@ Commander::run()

 		/* Check for failure detector status */
 		if (_failure_detector.update(_status, _vehicle_control_mode)) {
+			const bool motor_failure_changed = ((_status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) > 0) !=
+							   _failure_detector.getStatus().flags.motor;
 			_status.failure_detector_status = _failure_detector.getStatus().value;
 			auto fd_status_flags = _failure_detector.getStatusFlags();
 			_status_changed = true;
@@ -2904,6 +2843,67 @@ Commander::run()
 								 (imbalanced_propeller_action_t)_param_com_imb_prop_act.get());
 				}
 			}
+
+			// One-time actions based on motor failure
+			if (motor_failure_changed) {
+				if (fd_status_flags.motor) {
+					mavlink_log_critical(&_mavlink_log_pub, "Motor failure detected\t");
+					events::send(events::ID("commander_motor_failure"), events::Log::Emergency,
+						     "Motor failure! Land immediately");
+					set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, _status);
+
+				} else {
+					mavlink_log_critical(&_mavlink_log_pub, "Motor recovered\t");
+					events::send(events::ID("commander_motor_recovered"), events::Log::Warning,
+						     "Motor recovered, landing still advised");
+					set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, true, _status);
+				}
+			}
+
+			if (fd_status_flags.motor) {
+				switch ((ActuatorFailureActions)_param_com_actuator_failure_act.get()) {
+				case ActuatorFailureActions::AUTO_LOITER:
+					mavlink_log_critical(&_mavlink_log_pub, "Loitering due to actuator failure\t");
+					events::send(events::ID("commander_act_failure_loiter"), events::Log::Warning,
+						     "Loitering due to actuator failure");
+					main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
+					_status_changed = true;
+					break;
+
+				case ActuatorFailureActions::AUTO_LAND:
+					mavlink_log_critical(&_mavlink_log_pub, "Landing due to actuator failure\t");
+					events::send(events::ID("commander_act_failure_land"), events::Log::Warning,
+						     "Landing due to actuator failure");
+					main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, _internal_state);
+					_status_changed = true;
+					break;
+
+				case ActuatorFailureActions::AUTO_RTL:
+					mavlink_log_critical(&_mavlink_log_pub, "Returning home due to actuator failure\t");
+					events::send(events::ID("commander_act_failure_rtl"), events::Log::Warning,
+						     "Returning home due to actuator failure");
+					main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, _internal_state);
+					_status_changed = true;
+					break;
+
+				case ActuatorFailureActions::TERMINATE:
+					if (!_armed.manual_lockdown) {
+						mavlink_log_critical(&_mavlink_log_pub, "Flight termination due to actuator failure\t");
+						events::send(events::ID("commander_act_failure_term"), events::Log::Warning,
+							     "Flight termination due to actuator failure");
+						_status_changed = true;
+						_armed.manual_lockdown = true;
+						send_parachute_command();
+					}
+
+					break;
+
+				default:
+					// nothing to do here
+					break;
+				}
+
+			}
 		}

 		// Check wind speed actions if either high wind warning or high wind RTL is enabled
@@ -3090,7 +3090,9 @@ Commander::run()
 			fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs;
 			fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
 			fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
+			fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
 			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
+			fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
 			_failure_detector_status_pub.publish(fd_status);
 		}

@@ -3170,7 +3172,7 @@ Commander::run()

 		_was_armed = _armed.armed;

-		arm_auth_update(now, params_updated || param_init_forced);
+		arm_auth_update(now, params_updated);

 		px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);

@@ -3198,8 +3200,6 @@ Commander::get_circuit_breaker_params()
 			CBRK_USB_CHK_KEY);
 	_status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(),
 			CBRK_AIRSPD_CHK_KEY);
-	_status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled_by_val(_param_cbrk_enginefail.get(),
-			CBRK_ENGINEFAIL_KEY);
 	_status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(),
 			CBRK_FLIGHTTERM_KEY);
 	_status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled_by_val(_param_cbrk_velposerr.get(),
@@ -3440,7 +3440,6 @@ Commander::update_control_mode()
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
@@ -3822,18 +3821,17 @@ 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 || cp_enabled;
+	const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || _collision_prevention_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 && cp_enabled));
+	const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode
+					&& _collision_prevention_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,
@@ -3853,8 +3851,6 @@ void Commander::battery_status_check()
 	hrt_abstime oldest_update = hrt_absolute_time();
 	float worst_battery_time_s{NAN};

-	_battery_current = 0.0f;
-
 	for (auto &battery_sub : _battery_status_subs) {
 		int index = battery_sub.get_instance();
 		battery_status_s battery;
@@ -3940,8 +3936,6 @@ void Commander::battery_status_check()
 				worst_battery_time_s = battery.time_remaining_s;
 			}

-			// Sum up current from all batteries.
-			_battery_current += battery.current_filtered_a;
 		}
 	}

@@ -4407,6 +4401,7 @@ void Commander::esc_status_check()
 					esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
 					events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
 							events::px4::enums::suggested_action_t::none;
+					// TODO: use esc_status.esc[index].actuator_function as index after SYS_CTRL_ALLOC becomes default
 					events::send<uint8_t, events::px4::enums::suggested_action_t>(events::ID("commander_esc_offline"),
 							events::Log::Critical, "ESC{1} offline. {2}", index + 1, action);
 				}
@@ -64,7 +64,6 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/action_request.h>
-#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
@@ -184,6 +183,8 @@ private:

 	void checkWindSpeedThresholds();

+	void updateParameters();
+
 	DEFINE_PARAMETERS(

 		(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
@@ -222,9 +223,6 @@ 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,

@@ -241,9 +239,7 @@ private:
 		(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,

 		// Engine failure
-		(ParamFloat<px4::params::COM_EF_THROT>) _param_ef_throttle_thres,
-		(ParamFloat<px4::params::COM_EF_C2T>) _param_ef_current2throttle_thres,
-		(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
+		(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,

 		(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
 		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
@@ -257,27 +253,21 @@ 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,
@@ -289,6 +279,14 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};

+	enum class ActuatorFailureActions {
+		DISABLED = 0,
+		AUTO_LOITER = 1,
+		AUTO_LAND = 2,
+		AUTO_RTL = 3,
+		TERMINATE = 4,
+	};
+
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};

@@ -320,6 +318,8 @@ 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,7 +345,6 @@ private:

 	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	hrt_abstime	_battery_failsafe_timestamp{0};
-	float		_battery_current{0.0f};
 	uint8_t		_last_connected_batteries{0};
 	uint32_t	_last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {};
 	uint16_t	_last_battery_fault[battery_status_s::MAX_INSTANCES] {};
@@ -357,6 +356,7 @@ 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,7 +371,6 @@ private:

 	hrt_abstime	_boot_timestamp{0};
 	hrt_abstime	_last_disarmed_timestamp{0};
-	hrt_abstime	_timestamp_engine_healthy{0}; ///< absolute time when engine was healty
 	hrt_abstime	_overload_start{0};		///< time when CPU overload started

 	hrt_abstime _led_armed_state_toggle{0};
@@ -408,7 +407,6 @@ private:

 	// Subscriptions
 	uORB::Subscription					_action_request_sub {ORB_ID(action_request)};
-	uORB::Subscription					_actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
 	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription					_estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
@@ -128,49 +128,6 @@ PARAM_DEFINE_INT32(COM_HLDL_LOSS_T, 120);
 */
 PARAM_DEFINE_INT32(COM_HLDL_REG_T, 0);

-/**
- * Engine Failure Throttle Threshold
- *
- * Engine failure triggers only above this throttle value
- *
- * @group Commander
- * @unit norm
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.01
- */
-PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
-
-/**
- * Engine Failure Current/Throttle Threshold
- *
- * Engine failure triggers only below this current value
- *
- * @group Commander
- * @min 0.0
- * @max 50.0
- * @unit A/%
- * @decimal 2
- * @increment 1
- */
-PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
-
-/**
- * Engine Failure Time Threshold
- *
- * Engine failure triggers only if the throttle threshold and the
- * current to throttle threshold are violated for this time
- *
- * @group Commander
- * @unit s
- * @min 0.0
- * @max 60.0
- * @decimal 1
- * @increment 1
- */
-PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
-
 /**
 * RC loss time threshold
 *
@@ -893,6 +850,23 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
 */
 PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);

+/**
+ * Set the actuator failure failsafe mode
+ *
+ * Note: actuator failure needs to be enabled and configured via FD_ACT_*
+ * parameters.
+ *
+ * @min 0
+ * @max 3
+ * @value 0 Disabled
+ * @value 1 Hold mode
+ * @value 2 Land mode
+ * @value 3 Return mode
+ * @value 4 Terminate
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0);
+
 /**
 * Flag to enable obstacle avoidance.
 *
@@ -42,6 +42,123 @@

 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)
 {
@@ -49,6 +166,8 @@ 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) {
@@ -65,8 +184,19 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		_status.flags.ext = false;
 	}

-	if (_param_escs_en.get()) {
-		updateEscsStatus(vehicle_status);
+	// esc_status subscriber is shared between subroutines
+	esc_status_s esc_status;
+
+	if (_esc_status_sub.update(&esc_status)) {
+		_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_fd_imb_prop_thr.get() > 0) {
@@ -80,7 +210,7 @@ void FailureDetector::updateAttitudeStatus()
 {
 	vehicle_attitude_s attitude;

-	if (_vehicule_attitude_sub.update(&attitude)) {
+	if (_vehicle_attitude_sub.update(&attitude)) {

 		const matrix::Eulerf euler(matrix::Quatf(attitude.q));
 		const float roll(euler.phi());
@@ -127,22 +257,26 @@ void FailureDetector::updateExternalAtsStatus()
 	}
 }

-void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
+void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
 {
 	hrt_abstime time_now = hrt_absolute_time();

 	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-		esc_status_s esc_status;
+		const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
+		const int all_escs_armed_mask = (1 << limited_esc_count) - 1;
+		const bool is_all_escs_armed = (all_escs_armed_mask == esc_status.esc_armed_flags);

-		if (_esc_status_sub.update(&esc_status)) {
-			int all_escs_armed = (1 << esc_status.esc_count) - 1;
+		bool is_esc_failure = !is_all_escs_armed;

-			_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
-			_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
+		for (int i = 0; i < limited_esc_count; i++) {
+			is_esc_failure = is_esc_failure | (esc_status.esc[i].failures > 0);
+		}

-			if (_esc_failure_hysteresis.get_state()) {
-				_status.flags.arm_escs = true;
-			}
+		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
+		_esc_failure_hysteresis.set_state_and_update(is_esc_failure, time_now);
+
+		if (_esc_failure_hysteresis.get_state()) {
+			_status.flags.arm_escs = true;
 		}

 	} else {
@@ -209,3 +343,111 @@ void FailureDetector::updateImbalancedPropStatus()
 		}
 	}
 }
+
+void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
+{
+	// What need to be checked:
+	//
+	// 1. ESC telemetry disappears completely -> dead ESC or power loss on that ESC
+	// 2. ESC failures like overvoltage, overcurrent etc. But DShot driver for example is not populating the field 'esc_report.failures'
+	// 3. Motor current too low. Compare drawn motor current to expected value from a parameter
+	// -- ESC voltage does not really make sense and is highly dependent on the setup
+
+	// First wait for some ESC telemetry that has the required fields. Before that happens, don't check this ESC
+	// Then check
+
+	const hrt_abstime time_now = hrt_absolute_time();
+
+	// Only check while armed
+	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
+
+		actuator_motors_s actuator_motors{};
+		_actuator_motors_sub.copy(&actuator_motors);
+
+		// Check individual ESC reports
+		for (int esc_status_idx = 0; esc_status_idx < limited_esc_count; esc_status_idx++) {
+
+			const esc_report_s &cur_esc_report = esc_status.esc[esc_status_idx];
+
+			// Map the esc status index to the actuator function index
+			const unsigned i_esc = cur_esc_report.actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
+
+			if (i_esc >= actuator_motors_s::NUM_CONTROLS) {
+				continue;
+			}
+
+			// Check if ESC telemetry was available and valid at some point. This is a prerequisite for the failure detection.
+			if (!(_motor_failure_esc_valid_current_mask & (1 << i_esc)) && cur_esc_report.esc_current > 0.0f) {
+				_motor_failure_esc_valid_current_mask |= (1 << i_esc);
+			}
+
+			// Check for telemetry timeout
+			const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp;
+			const bool esc_timed_out = telemetry_age > 100_ms;  // TODO: magic number
+
+			const bool esc_was_valid = _motor_failure_esc_valid_current_mask & (1 << i_esc);
+			const bool esc_timeout_currently_flagged = _motor_failure_esc_timed_out_mask & (1 << i_esc);
+
+			if (esc_was_valid && esc_timed_out && !esc_timeout_currently_flagged) {
+				// Set flag
+				_motor_failure_esc_timed_out_mask |= (1 << i_esc);
+
+			} else if (!esc_timed_out && esc_timeout_currently_flagged) {
+				// Reset flag
+				_motor_failure_esc_timed_out_mask &= ~(1 << i_esc);
+			}
+
+			// Check if ESC current is too low
+			float esc_throttle = 0.f;
+
+			if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
+				esc_throttle = fabsf(actuator_motors.control[i_esc]);
+			}
+
+			const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
+			const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
+						     _param_fd_motor_current2throttle_thres.get();
+
+			if (throttle_above_threshold && current_too_low && !esc_timed_out) {
+				if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
+					_motor_failure_undercurrent_start_time[i_esc] = time_now;
+				}
+
+			} else {
+				if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
+					_motor_failure_undercurrent_start_time[i_esc] = 0;
+				}
+			}
+
+			if (_motor_failure_undercurrent_start_time[i_esc] != 0
+			    && (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
+			    && (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
+				// Set flag
+				_motor_failure_esc_under_current_mask |= (1 << i_esc);
+
+			} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
+
+		}
+
+		bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
+
+		if (critical_esc_failure && !(_status.flags.motor)) {
+			// Add motor failure flag to bitfield
+			_status.flags.motor = true;
+
+		} else if (!critical_esc_failure && _status.flags.motor) {
+			// Reset motor failure flag
+			_status.flags.motor = false;
+		}
+
+	} else { // Disarmed
+		// reset ESC bitfield
+		for (int i_esc = 0; i_esc < actuator_motors_s::NUM_CONTROLS; i_esc++) {
+			_motor_failure_undercurrent_start_time[i_esc] = 0;
+		}
+
+		_motor_failure_esc_under_current_mask = 0;
+		_status.flags.motor = false;
+	}
+}
@@ -51,9 +51,13 @@

 // 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>
@@ -69,26 +73,44 @@ 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);
+	void updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
+	void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
 	void updateImbalancedPropStatus();

 	failure_detector_status_u _status{};
@@ -103,11 +125,20 @@ private:
 	uint32_t _selected_accel_device_id{0};
 	hrt_abstime _imu_status_timestamp_prev{0};

-	uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
+	// Motor failure check
+	uint8_t _motor_failure_esc_valid_current_mask{};  // ESC 1-8, true if ESC telemetry was valid at some point
+	uint8_t _motor_failure_esc_timed_out_mask{};      // ESC telemetry no longer available -> failure
+	uint8_t _motor_failure_esc_under_current_mask{};  // ESC drawing too little current -> failure
+	hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};
+
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance
 	uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
+
+	FailureInjector _failure_injector;

 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@@ -117,6 +148,12 @@ private:
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
 		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
 		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
-		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
+		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
+
+		// Actuator failure
+		(ParamBool<px4::params::FD_ACT_EN>) _param_fd_actuator_en,
+		(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_motor_throttle_thres,
+		(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_motor_current2throttle_thres,
+		(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_motor_time_thres
 	)
 };
@@ -156,3 +156,59 @@ PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
 * @group Failure Detector
 */
 PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
+
+/**
+ * Enable Actuator Failure check
+ *
+ * If enabled, failure detector will verify that for motors, a minimum amount of ESC current per throttle
+ * level is being consumed.
+ * Otherwise this indicates an motor failure.
+ *
+ * @boolean
+ * @reboot_required true
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_ACT_EN, 1);
+
+/**
+ * Motor Failure Throttle Threshold
+ *
+ * Motor failure triggers only above this throttle value.
+ *
+ * @group Failure Detector
+ * @unit norm
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ */
+PARAM_DEFINE_FLOAT(FD_ACT_MOT_THR, 0.2f);
+
+/**
+ * Motor Failure Current/Throttle Threshold
+ *
+ * Motor failure triggers only below this current value
+ *
+ * @group Failure Detector
+ * @min 0.0
+ * @max 50.0
+ * @unit A/%
+ * @decimal 2
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 2.0f);
+
+/**
+ * Motor Failure Time Threshold
+ *
+ * Motor failure triggers only if the throttle threshold and the
+ * current to throttle threshold are violated for this time.
+ *
+ * @group Failure Detector
+ * @unit ms
+ * @min 10
+ * @max 10000
+ * @increment 100
+ */
+PARAM_DEFINE_INT32(FD_ACT_MOT_TOUT, 100);
@@ -383,9 +383,6 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_

 		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
-		} else if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
 		} else if (status_flags.vtol_transition_failure) {

 			set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
@@ -434,10 +431,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 	case commander_state_s::MAIN_STATE_AUTO_LOITER:

 		/* go into failsafe on a engine failure */
-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state

 		} else if (status_flags.vtol_transition_failure) {
@@ -475,10 +469,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_

 		/* require global position and home, also go into failsafe on an engine failure */

-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 		} else {
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@@ -488,12 +479,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_

 	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:

-		/* require global position and home */
+		// require global position and home

-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state

 		} else {
@@ -503,15 +491,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		break;

 	case commander_state_s::MAIN_STATE_ORBIT:
-		if (status.engine_failure) {
-			// failsafe: on engine failure
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-			// Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit
-			// is not possible and therefore the internal_state needs to be adjusted.
-			internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// failsafe: necessary position estimate lost; switching is done in check_invalid_pos_nav_state

 			// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
@@ -548,12 +528,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
 	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:

-		/* require local position */
+		// require local position

-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state

 		} else if (status_flags.vtol_transition_failure) {
@@ -590,12 +567,10 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_

 	case commander_state_s::MAIN_STATE_AUTO_LAND:

-		/* require local position */
+		// require local position

-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {

-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state

 		} else {
@@ -606,12 +581,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_

 	case commander_state_s::MAIN_STATE_AUTO_PRECLAND:

-		/* must be rotary wing plus same requirements as normal landing */
+		// must be rotary wing plus same requirements as normal landing

-		if (status.engine_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state

 		} else {
@@ -98,6 +98,15 @@ 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
 	 *
@@ -234,4 +243,5 @@ 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};
 };
@@ -59,7 +59,7 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 	if (_mix_update_needed) {
 		matrix::geninv(_effectiveness, _mix);

-		if (_normalization_needs_update) {
+		if (_normalization_needs_update && !_had_actuator_failure) {
 			updateControlAllocationMatrixScale();
 			_normalization_needs_update = false;
 		}
@@ -92,8 +92,8 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act
 	float k_max = 0.f;

 	for (int i = 0; i < _num_actuators; i++) {
-		// 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) {
+		// Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains
+		if (fabsf(desaturation_vector(i)) < 0.2f) {
 			continue;
 		}

@@ -205,7 +205,7 @@ ControlAllocator::update_allocation_method(bool force)
 bool
 ControlAllocator::update_effectiveness_source()
 {
-	EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
+	const EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();

 	if (_effectiveness_source_id != source) {

@@ -305,8 +305,12 @@ ControlAllocator::Run()
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);

-		updateParams();
-		parameters_updated();
+		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();
+		}
 	}

 	if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) {
@@ -376,6 +380,8 @@ 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)
@@ -503,6 +509,27 @@ 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]);
@@ -557,7 +584,8 @@ ControlAllocator::publish_control_allocator_status()
 	control_allocator_status.allocated_thrust[2] = allocated_control(5);

 	// Unallocated control
-	matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[0]->getControlSetpoint() - allocated_control;
+	const 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);
@@ -585,6 +613,9 @@ 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);
 }

@@ -604,7 +635,7 @@ ControlAllocator::publish_actuator_controls()
 	int actuator_idx = 0;
 	int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};

-	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors();
+	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask;

 	// motors
 	int motors_idx;
@@ -648,6 +679,56 @@ 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();
@@ -716,6 +797,11 @@ 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);

@@ -74,6 +74,7 @@
 #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
 {
@@ -128,6 +129,8 @@ private:

 	void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);

+	void check_for_motor_failures();
+
 	void publish_control_allocator_status();

 	void publish_actuator_controls();
@@ -152,6 +155,11 @@ 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

@@ -175,10 +183,15 @@ 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};
@@ -193,6 +206,7 @@ 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
 	)

@@ -436,6 +436,19 @@ 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:
@@ -459,12 +459,12 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	_current_task.task->setYawHandler(_wv_controller);

 	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
-	vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
+	trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
 	vehicle_constraints_s constraints = FlightTask::empty_constraints;

 	if (_current_task.task->updateInitialize() && _current_task.task->update()) {
 		// setpoints and constraints for the position controller from flighttask
-		setpoint = _current_task.task->getPositionSetpoint();
+		setpoint = _current_task.task->getTrajectorySetpoint();
 		constraints = _current_task.task->getConstraints();
 	}

@@ -504,7 +504,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	_old_landing_gear_position = landing_gear.landing_gear;
 }

-void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
+void FlightModeManager::limitAltitude(trajectory_setpoint_s &setpoint,
 				      const vehicle_local_position_s &vehicle_local_position)
 {
 	if (_param_lndmc_alt_max.get() < 0.0f || !_home_position_sub.get().valid_alt
@@ -518,8 +518,8 @@ void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoin

 	if (vehicle_local_position.z < min_z) {
 		// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
-		setpoint.z = min_z;
-		setpoint.vz = math::max(setpoint.vz, 0.f);
+		setpoint.position[2] = min_z;
+		setpoint.velocity[2] = math::max(setpoint.velocity[2], 0.f);
 	}
 }

@@ -531,11 +531,11 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 	}

 	// Save current setpoints for the next FlightTask
-	vehicle_local_position_setpoint_s last_setpoint = FlightTask::empty_setpoint;
+	trajectory_setpoint_s last_setpoint = FlightTask::empty_setpoint;
 	ekf_reset_counters_s last_reset_counters{};

 	if (isAnyTaskActive()) {
-		last_setpoint = _current_task.task->getPositionSetpoint();
+		last_setpoint = _current_task.task->getTrajectorySetpoint();
 		last_reset_counters = _current_task.task->getResetCounters();
 	}

@@ -47,6 +47,7 @@
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/takeoff_status.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
@@ -92,7 +93,7 @@ private:
 	void send_vehicle_cmd_do(uint8_t nav_state);
 	void handleCommand();
 	void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
-	void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
+	void limitAltitude(trajectory_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);

 	/**
 	 * Switch to a specific task (for normal usage)
@@ -152,7 +153,7 @@ private:
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};

 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
-	uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
+	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
@@ -48,7 +48,7 @@ FlightTaskAuto::FlightTaskAuto() :

 }

-bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);

@@ -61,8 +61,8 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp
 	_velocity_setpoint = _velocity;
 	_position_setpoint = _position;

-	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
-	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+	Vector3f vel_prev{last_setpoint.velocity};
+	Vector3f pos_prev{last_setpoint.position};
 	Vector3f accel_prev{last_setpoint.acceleration};

 	for (int i = 0; i < 3; i++) {
@@ -87,7 +87,7 @@ public:
 	FlightTaskAuto();

 	virtual ~FlightTaskAuto() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 	bool updateInitialize() override;
 	bool update() override;
@@ -36,7 +36,7 @@

 #include "FlightTaskDescend.hpp"

-bool FlightTaskDescend::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	// stay level to minimize horizontal drift
@@ -46,7 +46,7 @@ public:
 	virtual ~FlightTaskDescend() = default;

 	bool update() override;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;

 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
@@ -36,7 +36,7 @@

 #include "FlightTaskFailsafe.hpp"

-bool FlightTaskFailsafe::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskFailsafe::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;
@@ -47,7 +47,7 @@ public:

 	virtual ~FlightTaskFailsafe() = default;
 	bool update() override;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;

 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
@@ -4,12 +4,12 @@

 constexpr uint64_t FlightTask::_timeout;
 // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
-const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
+const trajectory_setpoint_s FlightTask::empty_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};

 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};

-bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
@@ -21,8 +21,8 @@ bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint
 void FlightTask::reActivate()
 {
 	// Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection
-	vehicle_local_position_setpoint_s setpoint_preserve_vertical{empty_setpoint};
-	setpoint_preserve_vertical.vz = _velocity_setpoint(2);
+	trajectory_setpoint_s setpoint_preserve_vertical{empty_setpoint};
+	setpoint_preserve_vertical.velocity[2] = _velocity_setpoint(2);
 	activate(setpoint_preserve_vertical);
 }

@@ -76,30 +76,20 @@ void FlightTask::_checkEkfResetCounters()
 	}
 }

-const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
+const trajectory_setpoint_s FlightTask::getTrajectorySetpoint()
 {
-	/* fill position setpoint message */
-	vehicle_local_position_setpoint_s vehicle_local_position_setpoint{};
-	vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
+	trajectory_setpoint_s trajectory_setpoint{};
+	trajectory_setpoint.timestamp = hrt_absolute_time();

-	vehicle_local_position_setpoint.x = _position_setpoint(0);
-	vehicle_local_position_setpoint.y = _position_setpoint(1);
-	vehicle_local_position_setpoint.z = _position_setpoint(2);
+	_position_setpoint.copyTo(trajectory_setpoint.position);
+	_velocity_setpoint.copyTo(trajectory_setpoint.velocity);
+	_acceleration_setpoint.copyTo(trajectory_setpoint.acceleration);
+	_jerk_setpoint.copyTo(trajectory_setpoint.jerk);

-	vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
-	vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
-	vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
+	trajectory_setpoint.yaw = _yaw_setpoint;
+	trajectory_setpoint.yawspeed = _yawspeed_setpoint;

-	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
-	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
-
-	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
-	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
-
-	// deprecated, only kept for output logging
-	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
-
-	return vehicle_local_position_setpoint;
+	return trajectory_setpoint;
 }

 void FlightTask::_resetSetpoints()
@@ -46,6 +46,7 @@
 #include <matrix/matrix/math.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/landing_gear.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
@@ -81,7 +82,7 @@ public:
 	 * @param last_setpoint last output of the previous task
 	 * @return true on success, false on error
 	 */
-	virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint);
+	virtual bool activate(const trajectory_setpoint_s &last_setpoint);

 	/**
 	 * Call this to reset an active Flight Task
@@ -112,7 +113,7 @@ public:
 	 * Get the output data
 	 * @return task output setpoints that get executed by the positon controller
 	 */
-	const vehicle_local_position_setpoint_s getPositionSetpoint();
+	const trajectory_setpoint_s getTrajectorySetpoint();

 	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
 	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
@@ -141,7 +142,7 @@ public:
 	 * Empty setpoint.
 	 * All setpoints are set to NAN.
 	 */
-	static const vehicle_local_position_setpoint_s empty_setpoint;
+	static const trajectory_setpoint_s empty_setpoint;

 	/**
 	 * Empty constraints.
@@ -43,21 +43,21 @@ FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
 	_stick_acceleration_xy(this)
 {};

-bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);

 	_stick_acceleration_xy.resetPosition();

-	if (PX4_ISFINITE(last_setpoint.vx) && PX4_ISFINITE(last_setpoint.vy)) {
-		_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.vx, last_setpoint.vy));
+	if (PX4_ISFINITE(last_setpoint.velocity[0]) && PX4_ISFINITE(last_setpoint.velocity[1])) {
+		_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.velocity));

 	} else {
 		_stick_acceleration_xy.resetVelocity(_velocity.xy());
 	}

 	if (PX4_ISFINITE(last_setpoint.acceleration[0]) && PX4_ISFINITE(last_setpoint.acceleration[1])) {
-		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1]));
+		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
 	}

 	return ret;
@@ -49,7 +49,7 @@ class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
 public:
 	FlightTaskManualAcceleration();
 	virtual ~FlightTaskManualAcceleration() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;

 	/**
@@ -60,7 +60,7 @@ bool FlightTaskManualAltitude::updateInitialize()
 	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }

-bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_yaw_setpoint = NAN;
@@ -49,7 +49,7 @@ class FlightTaskManualAltitude : public FlightTask
 public:
 	FlightTaskManualAltitude();
 	virtual ~FlightTaskManualAltitude() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;

@@ -41,17 +41,17 @@

 using namespace matrix;

-bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualAltitudeSmoothVel::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);

 	// Check if the previous FlightTask provided setpoints

 	// If the position setpoint is unknown, set to the current postion
-	float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2);
+	float z_sp_last = PX4_ISFINITE(last_setpoint.position[2]) ? last_setpoint.position[2] : _position(2);

 	// If the velocity setpoint is unknown, set to the current velocity
-	float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2);
+	float vz_sp_last = PX4_ISFINITE(last_setpoint.velocity[2]) ? last_setpoint.velocity[2] : _velocity(2);

 	// No acceleration estimate available, set to zero if the setpoint is NAN
 	float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f;
@@ -48,7 +48,7 @@ public:
 	FlightTaskManualAltitudeSmoothVel() = default;
 	virtual ~FlightTaskManualAltitudeSmoothVel() = default;

-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;

 protected:
 	virtual void _updateSetpoints() override;
@@ -56,7 +56,7 @@ bool FlightTaskManualPosition::updateInitialize()
 	       && PX4_ISFINITE(_velocity(1));
 }

-bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualPosition::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	// all requirements from altitude-mode still have to hold
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
@@ -49,7 +49,7 @@ public:
 	FlightTaskManualPosition();

 	virtual ~FlightTaskManualPosition() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;

 	/**
@@ -38,14 +38,14 @@

 using namespace matrix;

-bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualPosition::activate(last_setpoint);

 	// Check if the previous FlightTask provided setpoints
 	Vector3f accel_prev{last_setpoint.acceleration};
-	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
-	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+	Vector3f vel_prev{last_setpoint.velocity};
+	Vector3f pos_prev{last_setpoint.position};

 	for (int i = 0; i < 3; i++) {
 		// If the position setpoint is unknown, set to the current postion
@@ -53,7 +53,7 @@ public:

 	virtual ~FlightTaskManualPositionSmoothVel() = default;

-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	void reActivate() override;

 protected:
@@ -158,7 +158,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const
 	}
 }

-bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
 	_orbit_radius = _radius_min;
@@ -176,8 +176,8 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
 	      && PX4_ISFINITE(_velocity(1))
 	      && PX4_ISFINITE(_velocity(2));

-	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
-	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+	Vector3f pos_prev{last_setpoint.position};
+	Vector3f vel_prev{last_setpoint.velocity};
 	Vector3f accel_prev{last_setpoint.acceleration};

 	for (int i = 0; i < 3; i++) {
@@ -57,7 +57,7 @@ public:
 	virtual ~FlightTaskOrbit() = default;

 	bool applyCommandParameters(const vehicle_command_s &command) override;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;

 protected:
@@ -70,14 +70,14 @@ void FlightTaskTransition::updateParameters()
 	}
 }

-bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);

 	_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);

-	if (PX4_ISFINITE(last_setpoint.vz)) {
-		_vel_z_filter.reset(last_setpoint.vz);
+	if (PX4_ISFINITE(last_setpoint.velocity[2])) {
+		_vel_z_filter.reset(last_setpoint.velocity[2]);

 	} else {
 		_vel_z_filter.reset(_velocity(2));
@@ -54,7 +54,7 @@ public:
 	FlightTaskTransition();

 	virtual ~FlightTaskTransition() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;

@@ -581,9 +581,10 @@ void FixedwingAttitudeControl::Run()
 						_wheel_ctrl.reset_integrator();
 					}

-					/* throttle passed through if it is finite and if no engine failure was detected */
-					_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;
+					/* 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;

 					/* scale effort by battery status */
 					if (_param_fw_bat_scale_en.get() &&
@@ -2682,12 +2682,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 		_reinitialize_tecs = false;
 	}

-	if (_vehicle_status.engine_failure) {
-		/* Force the slow downwards spiral */
-		pitch_min_rad = radians(-1.0f);
-		pitch_max_rad = radians(5.0f);
-	}
-
 	/* No underspeed protection in landing mode */
 	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);

@@ -2748,9 +2742,6 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo
 		local_position_setpoint.acceleration[0] = NAN;
 		local_position_setpoint.acceleration[1] = NAN;
 		local_position_setpoint.acceleration[2] = NAN;
-		local_position_setpoint.jerk[0] = NAN;
-		local_position_setpoint.jerk[1] = NAN;
-		local_position_setpoint.jerk[2] = NAN;
 		local_position_setpoint.thrust[0] = _att_sp.thrust_body[0];
 		local_position_setpoint.thrust[1] = _att_sp.thrust_body[1];
 		local_position_setpoint.thrust[2] = _att_sp.thrust_body[2];
@@ -80,6 +80,7 @@
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/tecs_status.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -44,6 +44,7 @@

 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/hover_thrust_estimate.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/takeoff_status.h>
@@ -287,7 +287,9 @@ int LogWriterFile::hardfault_store_filename(const char *log_file)

 void LogWriterFile::stop_log(LogType type)
 {
+	lock();
 	_buffers[(int)type]._should_run = false;
+	unlock();
 	notify();
 }

@@ -312,8 +314,10 @@ int LogWriterFile::thread_start()
 void LogWriterFile::thread_stop()
 {
 	// this will terminate the main loop of the writer thread
-	_exit_thread = true;
+	lock();
+	_exit_thread.store(true);
 	_buffers[0]._should_run = _buffers[1]._should_run = false;
+	unlock();

 	notify();

@@ -335,10 +339,10 @@ void *LogWriterFile::run_helper(void *context)

 void LogWriterFile::run()
 {
-	while (!_exit_thread) {
+	while (!_exit_thread.load()) {
 		// Outer endless loop
 		// Wait for _should_run flag
-		while (!_exit_thread) {
+		while (!_exit_thread.load()) {
 			bool start = false;
 			pthread_mutex_lock(&_mtx);
 			pthread_cond_wait(&_cv, &_mtx);
@@ -350,7 +354,7 @@ void LogWriterFile::run()
 			}
 		}

-		if (_exit_thread) {
+		if (_exit_thread.load()) {
 			break;
 		}

@@ -480,7 +484,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) {
+			if (_buffers[0]._should_run || _buffers[1]._should_run) {
 				pthread_cond_wait(&_cv, &_mtx);
 			}
 		}
@@ -34,6 +34,7 @@
 #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>
@@ -205,8 +206,8 @@ private:

 	LogFileBuffer _buffers[(int)LogType::Count];

-	bool 		_exit_thread = false;
-	bool		_need_reliable_transfer = false;
+	px4::atomic_bool	_exit_thread{false};
+	bool			_need_reliable_transfer{false};
 	pthread_mutex_t		_mtx;
 	pthread_cond_t		_cv;
 	pthread_t _thread = 0;
@@ -32,6 +32,9 @@
 ****************************************************************************/

 #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>

@@ -64,6 +67,16 @@ 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
@@ -78,6 +91,53 @@ 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();
@@ -361,7 +421,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 = _param_mav_sys_id.get();
+	command.target_system = _system_id;
 	command.target_component = 100; // any camera

 	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
@@ -375,7 +435,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 = _param_mav_sys_id.get();
+	command.target_system = _system_id;
 	command.target_component = 100; // any camera

 	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
@@ -395,7 +455,7 @@ void ManualControl::send_video_command()
 		command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_START_CAPTURE;
 	}

-	command.target_system = _param_mav_sys_id.get();
+	command.target_system = _system_id;
 	command.target_component = 100; // any camera

 	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
@@ -46,6 +46,7 @@
 #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>
@@ -97,6 +98,8 @@ 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},
@@ -137,10 +140,13 @@ 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::MAV_SYS_ID>) _param_mav_sys_id
+		(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6
 	)

 	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};
 };
@@ -260,11 +260,6 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
 		break;

-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		break;
-
 	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
 		break;
@@ -1011,18 +1011,18 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 	    (mavlink_system.sysid == target_local_ned.target_system || target_local_ned.target_system == 0) &&
 	    (mavlink_system.compid == target_local_ned.target_component || target_local_ned.target_component == 0)) {

-		vehicle_local_position_setpoint_s setpoint{};
+		trajectory_setpoint_s setpoint{};

 		const uint16_t type_mask = target_local_ned.type_mask;

 		if (target_local_ned.coordinate_frame == MAV_FRAME_LOCAL_NED) {
-			setpoint.x = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? (float)NAN : target_local_ned.x;
-			setpoint.y = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? (float)NAN : target_local_ned.y;
-			setpoint.z = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? (float)NAN : target_local_ned.z;
+			setpoint.position[0] = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? (float)NAN : target_local_ned.x;
+			setpoint.position[1] = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? (float)NAN : target_local_ned.y;
+			setpoint.position[2] = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? (float)NAN : target_local_ned.z;

-			setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_local_ned.vx;
-			setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_local_ned.vy;
-			setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_local_ned.vz;
+			setpoint.velocity[0] = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_local_ned.vx;
+			setpoint.velocity[1] = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_local_ned.vy;
+			setpoint.velocity[2] = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_local_ned.vz;

 			setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_local_ned.afx;
 			setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_local_ned.afy;
@@ -1047,14 +1047,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t

 				const float yaw = matrix::Eulerf{R}(2);

-				setpoint.vx = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1);
-				setpoint.vy = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1);
-				setpoint.vz = velocity_body_sp(2);
+				setpoint.velocity[0] = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1);
+				setpoint.velocity[1] = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1);
+				setpoint.velocity[2] = velocity_body_sp(2);

 			} else {
-				setpoint.vx = NAN;
-				setpoint.vy = NAN;
-				setpoint.vz = NAN;
+				matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.velocity);
 			}

 			const bool ignore_acceleration = type_mask & (POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE |
@@ -1071,14 +1069,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 				acceleration_setpoint.copyTo(setpoint.acceleration);

 			} else {
-				setpoint.acceleration[0] = NAN;
-				setpoint.acceleration[1] = NAN;
-				setpoint.acceleration[2] = NAN;
+				matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.acceleration);
 			}

-			setpoint.x = NAN;
-			setpoint.y = NAN;
-			setpoint.z = NAN;
+			matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.position);

 		} else {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED coordinate frame %" PRIu8 " unsupported\t",
@@ -1088,19 +1082,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			return;
 		}

-		setpoint.thrust[0] = NAN;
-		setpoint.thrust[1] = NAN;
-		setpoint.thrust[2] = NAN;
-
 		setpoint.yaw      = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE)      ? (float)NAN : target_local_ned.yaw;
 		setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_local_ned.yaw_rate;


 		offboard_control_mode_s ocm{};
-		ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z);
-		ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz);
-		ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
-				   || PX4_ISFINITE(setpoint.acceleration[2]);
+		ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
+		ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
+		ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();

 		if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported\t");
@@ -1142,7 +1131,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 	    (mavlink_system.sysid == target_global_int.target_system || target_global_int.target_system == 0) &&
 	    (mavlink_system.compid == target_global_int.target_component || target_global_int.target_component == 0)) {

-		vehicle_local_position_setpoint_s setpoint{};
+		trajectory_setpoint_s setpoint{};

 		const uint16_t type_mask = target_global_int.type_mask;

@@ -1162,10 +1151,10 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 			// global -> local
 			const double lat = target_global_int.lat_int / 1e7;
 			const double lon = target_global_int.lon_int / 1e7;
-			global_local_proj_ref.project(lat, lon, setpoint.x, setpoint.y);
+			global_local_proj_ref.project(lat, lon, setpoint.position[0], setpoint.position[1]);

 			if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
-				setpoint.z = local_pos.ref_alt - target_global_int.alt;
+				setpoint.position[2] = local_pos.ref_alt - target_global_int.alt;

 			} else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
 				home_position_s home_position{};
@@ -1173,7 +1162,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t

 				if (home_position.valid_alt) {
 					const float alt = home_position.alt - target_global_int.alt;
-					setpoint.z = alt - local_pos.ref_alt;
+					setpoint.position[2] = alt - local_pos.ref_alt;

 				} else {
 					// home altitude required
@@ -1186,7 +1175,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t

 				if (vehicle_global_position.terrain_alt_valid) {
 					const float alt = target_global_int.alt + vehicle_global_position.terrain_alt;
-					setpoint.z = local_pos.ref_alt - alt;
+					setpoint.position[2] = local_pos.ref_alt - alt;

 				} else {
 					// valid terrain alt required
@@ -1202,34 +1191,27 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 			}

 		} else {
-			setpoint.x = NAN;
-			setpoint.y = NAN;
-			setpoint.z = NAN;
+			matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.position);
 		}

 		// velocity
-		setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_global_int.vx;
-		setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_global_int.vy;
-		setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_global_int.vz;
+		setpoint.velocity[0] = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_global_int.vx;
+		setpoint.velocity[1] = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_global_int.vy;
+		setpoint.velocity[2] = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_global_int.vz;

 		// acceleration
 		setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_global_int.afx;
 		setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_global_int.afy;
 		setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? (float)NAN : target_global_int.afz;

-		setpoint.thrust[0] = NAN;
-		setpoint.thrust[1] = NAN;
-		setpoint.thrust[2] = NAN;
-
 		setpoint.yaw      = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE)      ? (float)NAN : target_global_int.yaw;
 		setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_global_int.yaw_rate;


 		offboard_control_mode_s ocm{};
-		ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z);
-		ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz);
-		ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
-				   || PX4_ISFINITE(setpoint.acceleration[2]);
+		ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
+		ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
+		ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();

 		if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT force not supported\t");
@@ -95,6 +95,7 @@
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/tune_control.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -314,7 +315,7 @@ private:
 	uORB::Publication<vehicle_attitude_setpoint_s>		_fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
 	uORB::Publication<vehicle_global_position_s>		_global_pos_pub{ORB_ID(vehicle_global_position)};
 	uORB::Publication<vehicle_local_position_s>		_local_pos_pub{ORB_ID(vehicle_local_position)};
-	uORB::Publication<vehicle_local_position_setpoint_s>	_trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
+	uORB::Publication<trajectory_setpoint_s>		_trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_odometry_s>			_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 	uORB::Publication<vehicle_odometry_s>			_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_rates_setpoint_s>		_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
@@ -131,9 +131,6 @@ private:
 			    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
 				system_status = MAV_STATE_FLIGHT_TERMINATION;

-			} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
-				system_status = MAV_STATE_EMERGENCY;
-
 			} else if (vehicle_status_flags.calibration_enabled) {
 				system_status = MAV_STATE_CALIBRATING;
 			}
@@ -437,7 +437,7 @@ private:
 				msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
 			}

-			if (status.engine_failure) {
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) {
 				msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
 			}

@@ -141,7 +141,6 @@ private:
 			bool vehicle_in_auto_mode = (vehicle_status.timestamp > 0)
 						    && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
-							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
@@ -615,7 +615,6 @@ void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_se
 	setpoint.yaw = setpoint.yawspeed = NAN;
 	setpoint.vx = setpoint.vy = setpoint.vz = NAN;
 	setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
-	setpoint.jerk[0] = setpoint.jerk[1] = setpoint.jerk[2] = NAN;
 	setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
 }

@@ -57,6 +57,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -141,7 +141,10 @@ void PositionControl::_velocityControl(const float dt)
 {
 	// PID velocity control
 	Vector3f vel_error = _vel_sp - _vel;
-	Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
+	_velocity_proportional_portion = vel_error.emult(_gain_vel_p);
+	_velocity_integral_potion = _vel_int;
+	_velocity_derivative_portion = - _vel_dot.emult(_gain_vel_d);
+	Vector3f acc_sp_velocity = _velocity_proportional_portion + _velocity_integral_potion + _velocity_derivative_portion;

 	// No control input from setpoints or corresponding states which are NAN
 	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
@@ -240,14 +243,17 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	local_position_setpoint.x = _pos_sp(0);
 	local_position_setpoint.y = _pos_sp(1);
 	local_position_setpoint.z = _pos_sp(2);
-	local_position_setpoint.yaw = _yaw_sp;
-	local_position_setpoint.yawspeed = _yawspeed_sp;
 	local_position_setpoint.vx = _vel_sp(0);
 	local_position_setpoint.vy = _vel_sp(1);
 	local_position_setpoint.vz = _vel_sp(2);
 	_acc_sp.copyTo(local_position_setpoint.acceleration);
-	nans<3, 1>().copyTo(local_position_setpoint.jerk);
 	_thr_sp.copyTo(local_position_setpoint.thrust);
+	local_position_setpoint.yaw = _yaw_sp;
+	local_position_setpoint.yawspeed = _yawspeed_sp;
+
+	_velocity_proportional_portion.copyTo(local_position_setpoint.velocity_proportional_portion);
+	_velocity_integral_potion.copyTo(local_position_setpoint.velocity_integral_potion);
+	_velocity_derivative_portion.copyTo(local_position_setpoint.velocity_derivative_portion);
 }

 void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
@@ -216,4 +216,9 @@ private:
 	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
+
+	// Velocity PID portions for logging
+	matrix::Vector3f _velocity_proportional_portion;
+	matrix::Vector3f _velocity_integral_potion;
+	matrix::Vector3f _velocity_derivative_portion;
 };
@@ -52,7 +52,6 @@ TEST(PositionControlTest, EmptySetpoint)
 	EXPECT_FLOAT_EQ(output_setpoint.vy, 0.f);
 	EXPECT_FLOAT_EQ(output_setpoint.vz, 0.f);
 	EXPECT_EQ(Vector3f(output_setpoint.acceleration), Vector3f(0.f, 0.f, 0.f));
-	EXPECT_EQ(Vector3f(output_setpoint.jerk), Vector3f(NAN, NAN, NAN));
 	EXPECT_EQ(Vector3f(output_setpoint.thrust), Vector3f(0, 0, 0));

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

 #pragma once

-#include "enginefailure.h"
 #include "follow_target.h"
 #include "geofence.h"
 #include "land.h"
@@ -86,7 +85,7 @@ using namespace time_literals;
 /**
 * Number of navigation modes that need on_active/on_inactive calls
 */
-#define NAVIGATOR_MODE_ARRAY_SIZE 9
+#define NAVIGATOR_MODE_ARRAY_SIZE 8

 class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
@@ -393,7 +392,6 @@ private:
 	Land		_land;			/**< class for handling land commands */
 	PrecLand	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
-	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode (FW only!) */
 	FollowTarget	_follow_target;

 	NavigatorMode *_navigation_mode{nullptr};	/**< abstract pointer to current navigation mode class */
@@ -78,19 +78,17 @@ Navigator::Navigator() :
 	_land(this),
 	_precland(this),
 	_rtl(this),
-	_engineFailure(this),
 	_follow_target(this)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
-	_navigation_mode_array[3] = &_engineFailure;
-	_navigation_mode_array[4] = &_takeoff;
-	_navigation_mode_array[5] = &_land;
-	_navigation_mode_array[6] = &_precland;
-	_navigation_mode_array[7] = &_vtol_takeoff;
-	_navigation_mode_array[8] = &_follow_target;
+	_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;

 	_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
 	_handle_reverse_delay = param_find("VT_B_REV_DEL");
@@ -697,11 +695,6 @@ void Navigator::run()
 			_precland.set_mode(PrecLandMode::Required);
 			break;

-		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
-			_pos_sp_triplet_published_invalid_once = false;
-			navigation_mode_new = &_engineFailure;
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = &_follow_target;
@@ -333,7 +333,7 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 void
 RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
 {
-	const Vector3f desired_velocity{_trajectory_setpoint.vx, _trajectory_setpoint.vy, _trajectory_setpoint.vz};
+	const Vector3f desired_velocity{_trajectory_setpoint.velocity};
 	float dt = 0.01; // Using non zero value to a avoid division by zero

 	const float mission_throttle = _param_throttle_cruise.get();
@@ -436,10 +436,10 @@ RoverPositionControl::Run()

 				// local -> global
 				_global_local_proj_ref.reproject(
-					_trajectory_setpoint.x, _trajectory_setpoint.y,
+					_trajectory_setpoint.position[0], _trajectory_setpoint.position[1],
 					_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);

-				_pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.z;
+				_pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.position[2];
 				_pos_sp_triplet.current.valid = true;
 			}

@@ -64,6 +64,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -71,7 +72,6 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_torque_setpoint.h>
@@ -127,7 +127,7 @@ private:
 	vehicle_local_position_s		_local_pos{};			/**< global vehicle position */
 	actuator_controls_s				_act_controls{};		/**< direct control of actuators */
 	vehicle_attitude_s				_vehicle_att{};
-	vehicle_local_position_setpoint_s       _trajectory_setpoint{};
+	trajectory_setpoint_s _trajectory_setpoint{};
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};

@@ -56,9 +56,12 @@
 #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>
@@ -190,6 +193,7 @@ 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)};

@@ -232,6 +236,7 @@ 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);

@@ -259,12 +264,14 @@ 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] {};
@@ -293,6 +300,8 @@ 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
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Matthias Grob	527bdbb783	PositionControl: log individual portions of PID velocity control	2022-05-12 17:21:20 +02:00
Matthias Grob	57a0289627	trajectory_setpoint: correct comment typo "kinematically" Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>	2022-05-12 17:19:48 +02:00
Matthias Grob	4017f4bb0b	vehicle_local_position_setpoint: reorder fields for clarity	2022-05-12 17:19:48 +02:00
Matthias Grob	b67fbac296	uuv_pos_control: siplify passing on trajectory position setpoint	2022-05-12 17:19:48 +02:00
Matthias Grob	200124f094	mavlink_receiver: check entire Vectors for NAN Note: Behavioral change Inf also results in the flag being true.	2022-05-12 17:19:48 +02:00
Matthias Grob	8ca28f3796	Separate message for trajectory setpoint	2022-05-12 17:19:48 +02:00
Beat Küng	9166b6953d	output drivers: init SmartLock after exit_and_cleanup This fixes an invalid memory access when exiting the module: exit_and_cleanup destroys the object, but lock_guard is destructed after and accesses the lock.	2022-05-12 08:16:35 -04:00
Beat Küng	0a9378e0f6	mavsdk_tests: ensure motor is stopped for motor failure test	2022-05-12 07:58:56 +02:00
Alessandro Simovic	b5a3c58a95	sitl: loosen some timeouts The typhoon_h480 model would not always complete takeoff in 30 seconds or finish the mission within 60 seconds.	2022-05-12 07:58:56 +02:00
Alessandro Simovic	5c1932dcca	disable engine failure detector for SITL VTOL Tried increasing ESC timeout for VTOL first VTOL sitl tests are failing because the ESC telemetry seems to be coming in at 0.3 Hz	2022-05-12 07:58:56 +02:00
Alessandro Simovic	06f69cd469	Add SITL tests for control allocation	2022-05-12 07:58:56 +02:00
Beat Küng	0f860045f7	failure_detector: add failure injector class for motor telemetry This allows to test motor failures via 'failure motor off -i 1' on a real system.	2022-05-12 07:58:56 +02:00
Alessandro Simovic	4640f395d7	simulator_mavlink: Add ESC telemetry	2022-05-12 07:58:56 +02:00
Alessandro Simovic	20ccfbb719	control_allocator: remove failed motor from effectiveness - limit to handling only 1 motor failure - Log which motor failures are handled - Remove motor from effectiveness matrix without recomputing the scale / normalization	2022-05-12 07:58:56 +02:00
Alessandro Simovic	fb71e7587c	failure_detector: add motor/ESC failure detection	2022-05-12 07:58:56 +02:00
Beat Küng	dfd934fbdb	esc_report: add actuator_function	2022-05-12 07:58:56 +02:00
Hamish Willee	aab2feb8f5	pwm.cpp: remove the test example (#19587 )	2022-05-12 07:56:13 +02:00
alexklimaj	0f69f8ced8	Fix uavcan battery causing immediate RTL time remaining low	2022-05-11 21:48:12 -04:00
Daniel Agar	fba7c972d1	drivers/rc_input: ensure RC inversion is disabled initially and latch RC_INPUT_PROTO conservatively - this allows jumping straight to a non-SBUS RC protocol - increased the scan time per protocol 300->500 ms, which the newer DSM parser seems to need in some cases. - only set RC_INPUT_PROTO if we've had a successful RC lock for > 3 seconds	2022-05-11 14:30:41 -04:00
Daniel Agar	c772e5230f	commander: remove compile time dependencies on non-commander parameters - this allows builds with mavlink fully disabled - move commander MAN_ARM_GESTURE, RC_MAP_ARM_SW, MC_AIRMODE checks to manual_control	2022-05-11 10:14:23 -04:00
Beat Küng	8d36ba6727	log_writer_file: fix corner case when mission log is enabled Normally _should_run for mission is only ever true if _should_run for the normal log is. There are exceptions though: - the log buffer fails to allocate - there was a write failure (e.g. due to SD card removal) In that situation, the writer would not wait anymore but busy-loop.	2022-05-11 10:06:35 -04:00
Beat Küng	ebbe08bc86	log_writer_file: protect access to _should_run, use px4::atomicbool for _exit_thread	2022-05-11 10:06:35 -04:00
Peter van der Perk	0053aeec97	Cyphal restore O1Heap statistics	2022-05-11 09:49:18 -04:00
Peter van der Perk	e62e8b58d1	Update UCANS32K146 clocking to use XTAL and support higher pheriphal freq	2022-05-11 06:02:39 -07:00