CA testing hacks, external failure message, sitl tuning, disabling of rate controller saturation

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -21,7 +21,6 @@ param set-default VT_B_TRANS_DUR 5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
 

ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol

@@ -15,17 +15,17 @@ param set-default FD_ACT_MOT_TOUT 500
 param set-default CA_AIRFRAME 2
 
 param set-default CA_ROTOR_COUNT 5
-param set-default CA_ROTOR0_PX 0.1515
-param set-default CA_ROTOR0_PY 0.245
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR1_PX -0.1515
-param set-default CA_ROTOR1_PY -0.1875
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
 param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR2_PX 0.1515
-param set-default CA_ROTOR2_PY -0.245
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
 param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.1515
-param set-default CA_ROTOR3_PY 0.1875
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
 param set-default CA_ROTOR3_KM -0.05
 param set-default CA_ROTOR4_AX 1
 param set-default CA_ROTOR4_AZ 0
@@ -33,9 +33,9 @@ param set-default CA_ROTOR4_PX 0.2
 
 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1
-param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TRQ_R -1
 param set-default CA_SV_CS1_TYPE 2
-param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TRQ_R 1
 param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS2_TRQ_P 1
 param set-default PWM_MAIN_FUNC1 101
@@ -79,3 +79,18 @@ param set-default VT_FWD_THRUST_SC 1
 param set-default VT_F_TRANS_THR 0.75
 param set-default VT_TYPE 2
 
+param set-default CA_FW_DTHR_SC_R 1
+param set-default CA_FAILURE_MODE 1
+
+# Two ways to lock servo1 and servo2, either in allocation (will automatically notify about the failure), in in the output mixer
+# Only enable one of the two options
+
+# output mixer (NO automatic failure notification)
+# param set-default OUT_SRV_FAIL_IPT 1
+# param set-default OUT_SRV_FAIL_NR 2
+
+# allocation (automatic failure notification)
+param set-default COM_SRV_FAIL_IPT 1
+param set-default COM_SRV_FAIL_NR 2
+
+param set-default CA_FW_DTHR_AIRMD 1 # has to be enabled otheriwse the MC motors have no differential thrust capability

ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter

@@ -70,7 +70,6 @@ param set-default MC_PITCH_P 3
 param set-default VT_ARSP_TRANS 10
 param set-default VT_B_TRANS_DUR 5
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 1
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_TYPE 0
 

ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter

@@ -67,7 +67,6 @@ param set-default MC_PITCHRATE_P 0.3
 param set-default VT_ARSP_TRANS 15
 param set-default VT_B_TRANS_DUR 5
 param set-default VT_FW_DIFTHR_EN 7
-param set-default VT_FW_DIFTHR_S_Y 1
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_TYPE 0
 

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -25,7 +25,6 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
 

msg/CMakeLists.txt

@@ -93,6 +93,7 @@ set(msg_files
 	Event.msg
 	FigureEightStatus.msg
 	FailsafeFlags.msg
+	FailureDetectorExtServo.msg
 	FailureDetectorStatus.msg
 	FlightPhaseEstimation.msg
 	FollowTarget.msg

msg/ControlAllocatorStatus.msg

@@ -1,5 +1,8 @@
 uint64 timestamp                        # time since system start (microseconds)
 
+float32[3] torque_setpoint              # Current torque setpoint (normalized) for specific allocation matrix.
+float32[3] thrust_setpoint              # Current thrust setpoint (normalized) for specific allocation matrix.
+
 bool torque_setpoint_achieved           # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
 float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
                                         # Computed as: unallocated_torque = torque_setpoint - allocated_torque

msg/FailureDetectorExtServo.msg

@@ -0,0 +1,5 @@
+uint64 timestamp                    # time since system start (microseconds)
+
+bool fd_servo
+
+uint16 servo_failure_mask           # Bit-mask with servo indices, indicating critical servo failures

msg/FailureDetectorStatus.msg

@@ -9,6 +9,10 @@ bool fd_arm_escs
 bool fd_battery
 bool fd_imbalanced_prop
 bool fd_motor
+bool fd_servo
 
 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
+uint16 servo_failure_mask           # Bit-mask with servo indices, indicating critical servo failures
+
+uint16 servo_to_center_mask	    # HACK to test allocation without some servos

src/lib/matrix/matrix/Matrix.hpp

@@ -387,7 +387,7 @@ public:
 		}
 	}
 
-	void print(float eps = 1e-9) const
+	void print_symmetric(float eps = 1e-9) const
 	{
 		// print column numbering
 		if (N > 1) {
@@ -435,6 +435,45 @@ public:
 		}
 	}
 
+	void print(float eps = 1e-9) const
+	{
+		// print column numbering
+		if (N > 1) {
+			printf("  ");
+
+			for (unsigned i = 0; i < N; i++) {
+				printf("|%2u      ", i);
+
+			}
+
+			printf("\n");
+		}
+
+		const Matrix<Type, M, N> &self = *this;
+
+		for (unsigned i = 0; i < M; i++) {
+			printf("%2u|", i); // print row numbering
+
+			for (unsigned j = 0; j < N; j++) {
+				double d = static_cast<double>(self(i, j));
+
+				// avoid -0.0 for display
+				if (fabs(d - 0.0) < (double)eps) {
+					// print fixed width zero
+					printf(" 0       ");
+
+				} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
+					printf("% .1e ", d);
+
+				} else {
+					printf("% 6.5f ", d);
+				}
+			}
+
+			printf("\n");
+		}
+	}
+
 	Matrix<Type, N, M> transpose() const
 	{
 		Matrix<Type, N, M> res;

src/lib/matrix/matrix/Slice.hpp

@@ -351,6 +351,21 @@ public:
 		return min_val;
 	}
 
+	// sum of absolute values
+	Type sum_abs() const
+	{
+		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
+		Type accum(0);
+
+		for (size_t i = 0; i < P; i++) {
+			for (size_t j = 0; j < Q; j++) {
+				accum += std::abs(self(i, j));
+			}
+		}
+
+		return accum;
+	}
+
 private:
 	size_t _x0, _y0;
 	MatrixT *_data;

src/lib/mixer_module/mixer_module.cpp

@@ -417,6 +417,28 @@ bool MixingOutput::update()
 		_throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode;
 	}
 
+	manual_control_setpoint_s manual_control_setpoint;
+
+	if (_manual_control_sp_sub.update(&manual_control_setpoint)) {
+
+		switch (_param_out_srv_fail_ipt.get()) {
+		case 0:
+			// disable
+			_servo_locking_injection_active = false;
+			break;
+
+		case 1:
+			// yaw stick above 60% to enable
+			_servo_locking_injection_active = manual_control_setpoint.yaw > 0.6f;
+			break;
+
+		case 2:
+			// aux1 above 60% to enable
+			_servo_locking_injection_active = manual_control_setpoint.aux1 > 0.6f;
+			break;
+		}
+	}
+
 	// only used for sitl with lockstep
 	bool has_updates = _subscription_callback && _subscription_callback->updated();
 
@@ -442,7 +464,21 @@ bool MixingOutput::update()
 			all_disabled = false;
 
 			if (_armed.armed || (_armed.prearmed && _functions[i]->allowPrearmControl())) {
-				outputs[i] = _functions[i]->value(_function_assignment[i]);
+
+				float factor = 1.f;
+
+				if (_servo_locking_injection_active) {
+					if (_function_assignment[i] == OutputFunction::Servo1 && (_param_out_srv_fail_nr.get() == 0
+							|| _param_out_srv_fail_nr.get() == 2)) {
+						factor = 0.f;
+
+					} else if (_function_assignment[i] == OutputFunction::Servo2 && (_param_out_srv_fail_nr.get() == 1
+							|| _param_out_srv_fail_nr.get() == 2)) {
+						factor = 0.f;
+					}
+				}
+
+				outputs[i] = _functions[i]->value(_function_assignment[i]) * factor;
 
 			} else {
 				outputs[i] = NAN;

src/lib/mixer_module/mixer_module.hpp

@@ -59,6 +59,7 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/manual_control_setpoint.h>
 
 using namespace time_literals;
 
@@ -258,6 +259,9 @@ private:
 
 	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
 
+	uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};
+	bool _servo_locking_injection_active{false};
+
 	uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};
 
 	actuator_armed_s _armed{};
@@ -296,6 +300,8 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,   ///< multicopter air-mode
 		(ParamFloat<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max,
-		(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac ///< thrust to motor control signal modelling factor
+		(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor
+		(ParamInt<px4::params::OUT_SRV_FAIL_IPT>) _param_out_srv_fail_ipt,
+		(ParamInt<px4::params::OUT_SRV_FAIL_NR>) _param_out_srv_fail_nr
 	)
 };

src/lib/mixer_module/params.c

@@ -16,3 +16,23 @@
  * @group Mixer Output
  */
 PARAM_DEFINE_INT32(MC_AIRMODE, 0);
+
+/**
+ * Manual control source to inject servo failure
+ *
+ * @group Mixer Output
+ * @value 0 Disabled
+ * @value 1 yaw stick
+ * @value 2 aux1
+ */
+PARAM_DEFINE_INT32(OUT_SRV_FAIL_IPT, 0);
+
+/**
+ * Index of the servos to fail
+ *
+ * @group Mixer Output
+ * @value 0 Lock servo 1
+ * @value 1 Lock servo 2
+ * @value 2 Lock servo 1 and 2
+ */
+PARAM_DEFINE_INT32(OUT_SRV_FAIL_NR, 0);

src/lib/rate_control/rate_control.cpp

@@ -52,6 +52,10 @@ void RateControl::setSaturationStatus(const Vector3<bool> &saturation_positive,
 {
 	_control_allocator_saturation_positive = saturation_positive;
 	_control_allocator_saturation_negative = saturation_negative;
+
+	const Vector3<bool> saturation_disabled(false, false, false);
+	_control_allocator_saturation_positive = saturation_disabled;
+	_control_allocator_saturation_negative = saturation_disabled;
 }
 
 void RateControl::setPositiveSaturationFlag(size_t axis, bool is_saturated)

src/modules/commander/Commander.cpp

@@ -74,6 +74,8 @@
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/tune_control.h>
 
+using namespace time_literals;
+
 typedef enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED  = 1,   /* 0b00000001 Reserved for future use. | */
 	VEHICLE_MODE_FLAG_TEST_ENABLED         = 2,   /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
@@ -1907,6 +1909,77 @@ void Commander::run()
 			_vehicle_status.timestamp = hrt_absolute_time();
 			_vehicle_status_pub.publish(_vehicle_status);
 
+			// HACK: inject aileron failure (stuck to center) through here. Assume servo 0 is an aileron.
+			manual_control_setpoint_s manual_control_setpoint;
+			_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+			bool aileron_failure_injected = false;
+			bool servo_failure_detected = false;
+			int failed_servo_bitmask = 0;
+
+			if (!_param_com_srv_fail_cl.get()) {
+
+				if (_param_com_srv_fail_ipt.get() == 1) {
+
+					aileron_failure_injected =  manual_control_setpoint.yaw > 0.6f;
+
+				} else if (_param_com_srv_fail_ipt.get() == 2) {
+					aileron_failure_injected =  manual_control_setpoint.aux1 > 0.6f;
+				}
+
+				if (aileron_failure_injected) {
+					if (_time_failure_injected == 0) {
+						_time_failure_injected = hrt_absolute_time();
+					}
+
+					servo_failure_detected = hrt_elapsed_time(&_time_failure_injected) > 1_s; // trigger detection 2s after injection
+
+				} else {
+					_time_failure_injected = 0;
+					servo_failure_detected = false;
+				}
+
+				if (_param_com_srv_fail_nr.get() == 0) {
+					failed_servo_bitmask = 1 << 0;
+
+				} else if (_param_com_srv_fail_nr.get() == 1) {
+					failed_servo_bitmask = 1 << 1;
+
+				} else if (_param_com_srv_fail_nr.get() == 2) {
+					failed_servo_bitmask = (1 << 0) + (1 << 1);
+				}
+
+			} else {
+				_time_failure_injected = 0; // reset open loop variable
+				aileron_failure_injected = false;
+
+				if (_failure_detector_ext_servo.updated()) {
+					_failure_detector_ext_servo.update();
+					_time_last_ext_fail_topic = now;
+
+				}
+
+				if ((_time_last_ext_fail_topic > 0UL) && ((now - _time_last_ext_fail_topic) < 500_ms)) {
+					// aileron_failure_injected = _failure_detector_ext_servo.get().fd_servo;
+					servo_failure_detected = _failure_detector_ext_servo.get().fd_servo;
+					failed_servo_bitmask = _failure_detector_ext_servo.get().servo_failure_mask;
+				}
+			}
+
+			// Inform operator about detected servo failures
+			if (servo_failure_detected && failed_servo_bitmask > _reported_servo_fail) {
+				_reported_servo_fail = failed_servo_bitmask;
+
+				if ((_reported_servo_fail == (1 << 0)) || (_reported_servo_fail == (1 << 1))) {
+					events::send(events::ID("single_aileron_failure"), events::Log::Critical,
+						     "Actuator failure detected: single aileron fault");
+
+				} else if (_reported_servo_fail == ((1 << 1) + (1 << 0))) {
+					events::send(events::ID("double_aileron_failure"), events::Log::Critical,
+						     "Actuator failure detected: double aileron fault");
+				}
+			}
+
 			// failure_detector_status publish
 			failure_detector_status_s fd_status{};
 			fd_status.fd_roll = _failure_detector.getStatusFlags().roll;
@@ -1917,8 +1990,11 @@ void Commander::run()
 			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.fd_servo = servo_failure_detected;
 			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
 			fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
+			fd_status.servo_failure_mask = servo_failure_detected ? failed_servo_bitmask : 0;
+			fd_status.servo_to_center_mask = aileron_failure_injected ? failed_servo_bitmask : 0;
 			fd_status.timestamp = hrt_absolute_time();
 			_failure_detector_status_pub.publish(fd_status);
 		}

src/modules/commander/Commander.hpp

@@ -69,6 +69,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/failure_detector_ext_servo.h>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission_result.h>
@@ -280,6 +281,10 @@ private:
 	bool _have_taken_off_since_arming{false};
 	bool _status_changed{true};
 
+	hrt_abstime _time_failure_injected{0};
+	hrt_abstime _time_last_ext_fail_topic{0U};
+	uint16_t _reported_servo_fail{UINT16_C(0)};
+
 	vehicle_land_detected_s	_vehicle_land_detected{};
 
 	// commander publications
@@ -308,6 +313,7 @@ private:
 
 	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	uORB::SubscriptionData<failure_detector_ext_servo_s> 	_failure_detector_ext_servo{ORB_ID(failure_detector_ext_servo)};
 
 	// Publications
 	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
@@ -349,6 +355,9 @@ private:
 		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_takeoff_finished_action,
-		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
+		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max,
+		(ParamInt<px4::params::COM_SRV_FAIL_IPT>)   _param_com_srv_fail_ipt,
+		(ParamInt<px4::params::COM_SRV_FAIL_NR>)    _param_com_srv_fail_nr,
+		(ParamBool<px4::params::COM_SRV_FAIL_CL>)   _param_com_srv_fail_cl
 	)
 };

src/modules/commander/commander_params.c

@@ -1044,3 +1044,31 @@ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5);
  * @increment 1
  */
 PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);
+
+/**
+ * Manual control source to inject servo failure
+ *
+ * @group Commander
+ * @value 0 Disabled
+ * @value 1 yaw stick
+ * @value 2 aux1
+ */
+PARAM_DEFINE_INT32(COM_SRV_FAIL_IPT, 0);
+
+/**
+ * Index of the servos to fail
+ *
+ * @group Commander
+ * @value 0 Lock servo 0
+ * @value 1 Lock servo 1
+ * @value 2 Lock servo 1 and 2
+ */
+PARAM_DEFINE_INT32(COM_SRV_FAIL_NR, 0);
+
+/**
+ * Flag if closed loop servo detection and allocation shall be used.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_SRV_FAIL_CL, 0);

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp

@@ -87,6 +87,8 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const
 
 void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp)
 {
+	_stopped_motors_mask = 0; // TODO: as we have to reset here every iteration, there is no need to store this in the class
+
 	for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
 		const uint32_t motor_mask = (1u << actuator_idx);
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -63,7 +63,7 @@ enum class ActuatorType {
 enum class EffectivenessUpdateReason {
 	NO_EXTERNAL_UPDATE = 0,
 	CONFIGURATION_UPDATE = 1,
-	MOTOR_ACTIVATION_UPDATE = 2,
+	ACTUATOR_ACTIVATION_UPDATE = 2,
 };
 
 
@@ -134,6 +134,17 @@ public:
 		_flight_phase = flight_phase;
 	}
 
+	/**
+	 * Set flag to enable/disable auxiliary motors (effectiveness-type specific)
+	 *
+	 * Auxiliary motors are switched off (stopped) by default, but can be enabled
+	 * in certain cases (e.g. MC motors on a Standard VTOL in forward flight for
+	 * attitude control support).
+	 *
+	 * @param enable True to enable auxiliary motors, false to disable them.
+	 */
+	virtual void setEnableAuxiliaryMotors(bool enable) {}
+
 	/**
 	 * Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES.
 	 * This is expected to stay constant.
@@ -153,7 +164,7 @@ public:
 	/**
 	 * Query if the roll, pitch and yaw columns of the mixing matrix should be normalized
 	 */
-	virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const
+	virtual void getNormalizeAsPlanarMC(bool normalize[MAX_NUM_MATRICES]) const
 	{
 		for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
 			normalize[i] = false;
@@ -201,10 +212,20 @@ public:
 				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
 
 	/**
-	 * Get a bitmask of motors to be stopped
+	 * Get a bitmask of motors to be stopped.
+	 * Stopped motors are by definition NOT to be removed from the control allocation matrix,
+	 * but their output is to be stopped (setpoint=NAN).
+	 * Usually is used to not have motors spinning at 0 thrust.
 	 */
 	virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; }
 
+	/**
+	 * Get a bitmask of motors to be disabled.
+	 * Disabled motors are by definition to be removed from the control allocation matrix and
+	 * their output is to be stopped (setpoint=NAN).
+	 */
+	virtual uint32_t getDisabledMotors() const { return _disabled_motors_mask; }
+
 	/**
 	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
 	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
@@ -222,4 +243,5 @@ public:
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
 	uint32_t _stopped_motors_mask{0};
+	uint32_t _disabled_motors_mask{0};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp

@@ -55,7 +55,7 @@ TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve)
 	ActuatorEffectivenessHelicopter helicopter(nullptr, ActuatorType::MOTORS);
 	// run getEffectivenessMatrix with empty configuration to correctly initialize _first_swash_plate_servo_index
 	ActuatorEffectiveness::Configuration configuration{};
-	EffectivenessUpdateReason external_update = EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE;
+	EffectivenessUpdateReason external_update = EffectivenessUpdateReason::ACTUATOR_ACTIVATION_UPDATE;
 	helicopter.getEffectivenessMatrix(configuration, external_update);
 
 	Vector<float, 6> control_sp{};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -50,7 +50,7 @@ public:
 		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
 	}
 
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	void getNormalizeAsPlanarMC(bool normalize[MAX_NUM_MATRICES]) const override
 	{
 		normalize[0] = true;
 	}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -49,7 +49,7 @@ public:
 		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
 	}
 
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	void getNormalizeAsPlanarMC(bool normalize[MAX_NUM_MATRICES]) const override
 	{
 		normalize[0] = true;
 	}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -90,7 +90,7 @@ public:
 		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
 	}
 
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	void getNormalizeAsPlanarMC(bool normalize[MAX_NUM_MATRICES]) const override
 	{
 		normalize[0] = true;
 	}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -101,16 +101,26 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight
 
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
-	// update stopped motors
+	// update disabled motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors_mask |= _upwards_motors_mask;
+		_disabled_motors_mask |= _upwards_motors_mask;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
-		_stopped_motors_mask &= ~_upwards_motors_mask;
+		_disabled_motors_mask &= ~_upwards_motors_mask;
 		break;
 	}
 }
+
+void ActuatorEffectivenessStandardVTOL::setEnableAuxiliaryMotors(bool enable)
+{
+	if (enable) {
+		_disabled_motors_mask = 0;
+
+	} else {
+		_disabled_motors_mask = _upwards_motors_mask;
+	}
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -67,7 +67,7 @@ public:
 		allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
 	}
 
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	void getNormalizeAsPlanarMC(bool normalize[MAX_NUM_MATRICES]) const override
 	{
 		normalize[0] = true;
 		normalize[1] = false;
@@ -80,6 +80,7 @@ public:
 			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
+	void setEnableAuxiliaryMotors(bool enable) override;
 
 private:
 	ActuatorEffectivenessRotors _rotors;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -93,7 +93,13 @@ void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector<fl
 		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	if (matrix_index == 0) {
-		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+			stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+
+		} else {
+			// make sure all motors are un-stopped when out of forward flight
+			stopMaskedMotorsWithZeroThrust(0, actuator_sp);
+		}
 	}
 }
 
@@ -115,7 +121,12 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
 		_forwards_motors_mask = 0;
-		_stopped_motors_mask = 0;
+		_disabled_motors_mask = 0;
 		break;
 	}
 }
+
+void ActuatorEffectivenessTailsitterVTOL::setEnableAuxiliaryMotors(bool enable)
+{
+	// keep auxiliary motors disabled for now
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -64,7 +64,7 @@ public:
 		allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
 	}
 
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	void getNormalizeAsPlanarMC(bool normalize[MAX_NUM_MATRICES]) const override
 	{
 		normalize[0] = true;
 		normalize[1] = false;
@@ -78,6 +78,7 @@ public:
 
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
+	void setEnableAuxiliaryMotors(bool enable) override;
 
 	const char *name() const override { return "VTOL Tailsitter"; }
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -201,14 +201,20 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 
 			// in FW directly use throttle sp
 			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
-				for (int i = 0; i < _first_tilt_idx; ++i) {
-					actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
+				for (int motors_idx = 0; motors_idx < _first_tilt_idx; ++motors_idx) {
+					if (!(_disabled_motors_mask & (1 << motors_idx))) {
+						actuator_sp(motors_idx) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
+					}
 				}
 			}
 		}
 
 		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
 			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
+
+		} else {
+			// make sure all motors are un-stopped when out of forward flight
+			stopMaskedMotorsWithZeroThrust(0, actuator_sp);
 		}
 	}
 }
@@ -221,20 +227,25 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
-	// update stopped motors
+	// update disabled motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors_mask |= _untiltable_motors;
+		_disabled_motors_mask |= _untiltable_motors;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
-		_stopped_motors_mask = 0;
+		_disabled_motors_mask = 0;
 		break;
 	}
 }
 
+void ActuatorEffectivenessTiltrotorVTOL::setEnableAuxiliaryMotors(bool enable)
+{
+	// keep auxiliary motors disabled for now
+}
+
 void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
 {
 	// only handle matrix 0 (motors and tilts)

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -70,13 +70,14 @@ public:
 		allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
 	}
 
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	void getNormalizeAsPlanarMC(bool normalize[MAX_NUM_MATRICES]) const override
 	{
 		normalize[0] = true;
 		normalize[1] = false;
 	}
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
+	void setEnableAuxiliaryMotors(bool enable) override;
 
 	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp

@@ -49,7 +49,7 @@ public:
 		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
 	}
 
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	void getNormalizeAsPlanarMC(bool normalize[MAX_NUM_MATRICES]) const override
 	{
 		normalize[0] = true;
 	}

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -98,15 +98,6 @@ public:
 	 */
 	virtual void allocate() = 0;
 
-	/**
-	 * Set actuator failure flag
-	 * This prevents a change of the scaling in the matrix normalization step
-	 * in case of a motor failure.
-	 *
-	 * @param failure  Motor failure flag
-	 */
-	void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; }
-
 	/**
 	 * Set the control effectiveness matrix
 	 *
@@ -150,7 +141,7 @@ public:
 	 *
 	 * @return Effectiveness matrix
 	 */
-	const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const { return _effectiveness; }
+	const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrixAllocation() const { return _effectiveness; }
 
 	/**
 	 * Set the minimum actuator values
@@ -226,7 +217,11 @@ public:
 
 	int numConfiguredActuators() const { return _num_actuators; }
 
-	void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; }
+	void setNormalizeAsPlanarMC(bool normalize_as_mc) { _normalize_matrix_as_planar_mc = normalize_as_mc; }
+	void setAirmode(const int airmode) {_airmode = airmode; }
+
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> getMixMatrix() {return _mix; }
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> getNormalizedMixMatrix() {return _normalized_mix; }
 
 protected:
 	friend class ControlAllocator; // for _actuator_sp
@@ -242,6 +237,8 @@ protected:
 	matrix::Vector<float, NUM_AXES> _control_sp;   		///< Control setpoint
 	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};
+	bool _normalize_matrix_as_planar_mc{false};		///< if true, normalize roll, pitch and yaw columns optimized for planar MC
+	int _airmode{0};					///< 0: disabled, 1: RP airmode, 2: RPY airmode
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _normalized_mix;
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -59,7 +59,7 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 	if (_mix_update_needed) {
 		matrix::geninv(_effectiveness, _mix);
 
-		if (_normalization_needs_update && !_had_actuator_failure) {
+		if (_normalization_needs_update) {
 			updateControlAllocationMatrixScale();
 			_normalization_needs_update = false;
 		}
@@ -73,7 +73,7 @@ void
 ControlAllocationPseudoInverse::updateControlAllocationMatrixScale()
 {
 	// Same scale on roll and pitch
-	if (_normalize_rpy) {
+	if (_normalize_matrix_as_planar_mc) {
 
 		int num_non_zero_roll_torque = 0;
 		int num_non_zero_pitch_torque = 0;
@@ -107,34 +107,52 @@ ControlAllocationPseudoInverse::updateControlAllocationMatrixScale()
 		// Scale yaw separately
 		_control_allocation_scale(2) = _mix.col(2).max();
 
-	} else {
-		_control_allocation_scale(0) = 1.f;
-		_control_allocation_scale(1) = 1.f;
-		_control_allocation_scale(2) = 1.f;
-	}
+		// Scale thrust by the sum of the individual thrust axes, and use the scaling for the Z axis if there's no actuators
+		// (for tilted actuators)
+		_control_allocation_scale(THRUST_Z) = 1.f;
 
-	// Scale thrust by the sum of the individual thrust axes, and use the scaling for the Z axis if there's no actuators
-	// (for tilted actuators)
-	_control_allocation_scale(THRUST_Z) = 1.f;
+		for (int axis_idx = 2; axis_idx >= 0; --axis_idx) {
+			int num_non_zero_thrust = 0;
+			float norm_sum = 0.f;
 
-	for (int axis_idx = 2; axis_idx >= 0; --axis_idx) {
-		int num_non_zero_thrust = 0;
-		float norm_sum = 0.f;
+			for (int i = 0; i < _num_actuators; i++) {
+				float norm = fabsf(_mix(i, 3 + axis_idx));
+				norm_sum += norm;
 
-		for (int i = 0; i < _num_actuators; i++) {
-			float norm = fabsf(_mix(i, 3 + axis_idx));
-			norm_sum += norm;
+				if (norm > FLT_EPSILON) {
+					++num_non_zero_thrust;
+				}
+			}
 
-			if (norm > FLT_EPSILON) {
-				++num_non_zero_thrust;
+			if (num_non_zero_thrust > 0) {
+				_control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust;
+
+			} else {
+				_control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z);
 			}
 		}
 
-		if (num_non_zero_thrust > 0) {
-			_control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust;
+	} else {
+		// Fixed-wing with control surfaces normalization
+		for (int i = 0; i < NUM_AXES; i++) {
 
-		} else {
-			_control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z);
+			int num_non_zero_actuators = 0;
+
+			for (int j = 0; j < _num_actuators; j++) {
+
+				if (fabsf(_mix(j, i)) > 1e-3f) {
+					++num_non_zero_actuators;
+				}
+			}
+
+			const float axis_norm_scale = _mix.col(i).sum_abs();
+
+			if (num_non_zero_actuators > 0) {
+				_control_allocation_scale(i) = axis_norm_scale / num_non_zero_actuators;
+
+			} else {
+				_control_allocation_scale(i) = 1.f;
+			}
 		}
 	}
 }
@@ -142,27 +160,22 @@ ControlAllocationPseudoInverse::updateControlAllocationMatrixScale()
 void
 ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
 {
-	if (_control_allocation_scale(0) > FLT_EPSILON) {
-		_mix.col(0) /= _control_allocation_scale(0);
-		_mix.col(1) /= _control_allocation_scale(1);
-	}
+	// build the normalized matrix with the raw mixer matrix and the per-axis scales
+	_normalized_mix = _mix;
 
-	if (_control_allocation_scale(2) > FLT_EPSILON) {
-		_mix.col(2) /= _control_allocation_scale(2);
-	}
-
-	if (_control_allocation_scale(3) > FLT_EPSILON) {
-		_mix.col(3) /= _control_allocation_scale(3);
-		_mix.col(4) /= _control_allocation_scale(4);
-		_mix.col(5) /= _control_allocation_scale(5);
+	// TODO check for tilting motors if normalization is correct
+	for (int i = 0; i < NUM_AXES; i++) {
+		if (_control_allocation_scale(i) > FLT_EPSILON) {
+			_normalized_mix.col(i) = _mix.col(i) / _control_allocation_scale(i);
+		}
 	}
 
 	// Set all the small elements to 0 to avoid issues
 	// in the control allocation algorithms
 	for (int i = 0; i < _num_actuators; i++) {
 		for (int j = 0; j < NUM_AXES; j++) {
-			if (fabsf(_mix(i, j)) < 1e-3f) {
-				_mix(i, j) = 0.f;
+			if (fabsf(_normalized_mix(i, j)) < 1e-3f) {
+				_normalized_mix(i, j) = 0.f;
 			}
 		}
 	}
@@ -177,5 +190,5 @@ ControlAllocationPseudoInverse::allocate()
 	_prev_actuator_sp = _actuator_sp;
 
 	// Allocate
-	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
+	_actuator_sp = _actuator_trim + _normalized_mix * (_control_sp - _control_trim);
 }

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp

@@ -59,7 +59,6 @@ public:
 				    bool update_normalization_scale) override;
 
 protected:
-	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
 
 	bool _mix_update_needed{false};
 

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -49,7 +49,7 @@ ControlAllocationSequentialDesaturation::allocate()
 
 	_prev_actuator_sp = _actuator_sp;
 
-	switch (_param_mc_airmode.get()) {
+	switch (_airmode) {
 	case 1:
 		mixAirmodeRP();
 		break;
@@ -128,12 +128,14 @@ ControlAllocationSequentialDesaturation::mixAirmodeRP()
 
 	for (int i = 0; i < _num_actuators; i++) {
 		_actuator_sp(i) = _actuator_trim(i) +
-				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
-				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
-				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
-				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
-				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
-		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+				  _normalized_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _normalized_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _normalized_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(
+						  ControlAxis::THRUST_X)) +
+				  _normalized_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(
+						  ControlAxis::THRUST_Y)) +
+				  _normalized_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _normalized_mix(i, ControlAxis::THRUST_Z);
 	}
 
 	desaturateActuators(_actuator_sp, thrust_z);
@@ -153,14 +155,16 @@ ControlAllocationSequentialDesaturation::mixAirmodeRPY()
 
 	for (int i = 0; i < _num_actuators; i++) {
 		_actuator_sp(i) = _actuator_trim(i) +
-				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
-				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
-				  _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) +
-				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
-				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
-				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
-		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
-		yaw(i) = _mix(i, ControlAxis::YAW);
+				  _normalized_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _normalized_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _normalized_mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) +
+				  _normalized_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(
+						  ControlAxis::THRUST_X)) +
+				  _normalized_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(
+						  ControlAxis::THRUST_Y)) +
+				  _normalized_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _normalized_mix(i, ControlAxis::THRUST_Z);
+		yaw(i) = _normalized_mix(i, ControlAxis::YAW);
 	}
 
 	desaturateActuators(_actuator_sp, thrust_z);
@@ -182,14 +186,16 @@ ControlAllocationSequentialDesaturation::mixAirmodeDisabled()
 
 	for (int i = 0; i < _num_actuators; i++) {
 		_actuator_sp(i) = _actuator_trim(i) +
-				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
-				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
-				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
-				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
-				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
-		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
-		roll(i) = _mix(i, ControlAxis::ROLL);
-		pitch(i) = _mix(i, ControlAxis::PITCH);
+				  _normalized_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _normalized_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _normalized_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(
+						  ControlAxis::THRUST_X)) +
+				  _normalized_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(
+						  ControlAxis::THRUST_Y)) +
+				  _normalized_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _normalized_mix(i, ControlAxis::THRUST_Z);
+		roll(i) = _normalized_mix(i, ControlAxis::ROLL);
+		pitch(i) = _normalized_mix(i, ControlAxis::PITCH);
 	}
 
 	// only reduce thrust
@@ -211,9 +217,10 @@ ControlAllocationSequentialDesaturation::mixYaw()
 	ActuatorVector thrust_z;
 
 	for (int i = 0; i < _num_actuators; i++) {
-		_actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
-		yaw(i) = _mix(i, ControlAxis::YAW);
-		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+		_actuator_sp(i) += _normalized_mix(i,
+						   ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
+		yaw(i) = _normalized_mix(i, ControlAxis::YAW);
+		thrust_z(i) = _normalized_mix(i, ControlAxis::THRUST_Z);
 	}
 
 	// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp

@@ -121,8 +121,4 @@ private:
 	 * but yaw is decreased as much as required.
 	 */
 	void mixYaw();
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode   ///< air-mode
-	);
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -168,8 +168,8 @@ ControlAllocator::update_allocation_method(bool force)
 		AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES];
 		_actuator_effectiveness->getDesiredAllocationMethod(desired_methods);
 
-		bool normalize_rpy[ActuatorEffectiveness::MAX_NUM_MATRICES];
-		_actuator_effectiveness->getNormalizeRPY(normalize_rpy);
+		bool normalize_as_planar_mc[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		_actuator_effectiveness->getNormalizeAsPlanarMC(normalize_as_planar_mc);
 
 		for (int i = 0; i < _num_control_allocation; ++i) {
 			AllocationMethod method = configured_method;
@@ -197,7 +197,7 @@ ControlAllocator::update_allocation_method(bool force)
 				_num_control_allocation = 0;
 
 			} else {
-				_control_allocation[i]->setNormalizeRPY(normalize_rpy[i]);
+				_control_allocation[i]->setNormalizeAsPlanarMC(normalize_as_planar_mc[i]);
 				_control_allocation[i]->setActuatorSetpoint(actuator_sp[i]);
 			}
 		}
@@ -314,7 +314,7 @@ ControlAllocator::Run()
 #endif
 
 	// Check if parameters have changed
-	if (_parameter_update_sub.updated() && !_armed) {
+	if (_parameter_update_sub.updated()) {
 		// clear update
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
@@ -338,28 +338,26 @@ ControlAllocator::Run()
 
 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 
-			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
-
 			// Check if the current flight phase is HOVER or FIXED_WING
 			if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-				flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT;
+				_flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT;
 
 			} else {
-				flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
+				_flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
 			}
 
 			// Special cases for VTOL in transition
 			if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
 				if (vehicle_status.in_transition_to_fw) {
-					flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF;
+					_flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF;
 
 				} else {
-					flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF;
+					_flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF;
 				}
 			}
 
 			// Forward to effectiveness source
-			_actuator_effectiveness->setFlightPhase(flight_phase);
+			_actuator_effectiveness->setFlightPhase(_flight_phase);
 		}
 	}
 
@@ -402,9 +400,32 @@ ControlAllocator::Run()
 	if (do_update) {
 		_last_run = now;
 
-		check_for_motor_failures();
+		// update matrix if there was an actuator activation change (due to detected failure or actuator activation due to VTOL mode change)
+		if (check_for_actuator_activation_update()) {
+			update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::ACTUATOR_ACTIVATION_UPDATE);
 
-		update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
+		} else {
+			update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
+		}
+
+		float fw_dthr_weight[3] = {_param_ca_fw_dthr_wgt_r.get(), _param_ca_fw_dthr_wgt_p.get(), _param_ca_fw_dthr_wgt_y.get()};
+		float fw_dthr_scale[3] = {_param_ca_fw_dthr_sc_r.get(), _param_ca_fw_dthr_sc_p.get(), _param_ca_fw_dthr_sc_y.get()};
+
+		// VTOL Tailsitters have roll/yaw axis swaped in fixed-wing mode
+		if (_effectiveness_source_id == EffectivenessSource::TAILSITTER_VTOL) {
+			fw_dthr_weight[2] = fw_dthr_weight[0];
+			fw_dthr_weight[0] = fw_dthr_weight[2];
+			fw_dthr_scale[2] = fw_dthr_scale[0];
+			fw_dthr_scale[0] = fw_dthr_scale[2];
+		}
+
+		for (int i = 0; i < 3; i++) {
+			if (_handled_servo_failure_bitmask && _param_ca_fw_dthr_fb_en.get() && !_has_control_authority[1][i]) {
+				// If fallback is enabled and it is discovered that an axis doesn't have any
+				// control authority, then the differential thrust weight for that axis is set to 1.
+				fw_dthr_weight[i] = 1.f;
+			}
+		}
 
 		// Set control setpoint vector(s)
 		matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
@@ -415,11 +436,21 @@ ControlAllocator::Run()
 		c[0](4) = _thrust_sp(1);
 		c[0](5) = _thrust_sp(2);
 
+		const int airmode = _flight_phase != ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT ? _param_mc_airmode.get() :
+				    _param_ca_fw_dthr_airmd.get();
+
 		if (_num_control_allocation > 1) {
+			matrix::Vector<float, NUM_AXES> vehicle_torque_setpoint_matrix_1;
+
 			if (_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint)) {
-				c[1](0) = vehicle_torque_setpoint.xyz[0];
-				c[1](1) = vehicle_torque_setpoint.xyz[1];
-				c[1](2) = vehicle_torque_setpoint.xyz[2];
+				vehicle_torque_setpoint_matrix_1(0) = vehicle_torque_setpoint.xyz[0];
+				vehicle_torque_setpoint_matrix_1(1) = vehicle_torque_setpoint.xyz[1];
+				vehicle_torque_setpoint_matrix_1(2) = vehicle_torque_setpoint.xyz[2];
+
+				// reduce outputs on servos (matrix 1) if differential thrust with motors (matrix 0) is enabled
+				c[1](0) = vehicle_torque_setpoint_matrix_1(0) * (1.f - fw_dthr_weight[0]);
+				c[1](1) = vehicle_torque_setpoint_matrix_1(1) * (1.f - fw_dthr_weight[1]);
+				c[1](2) = vehicle_torque_setpoint_matrix_1(2) * (1.f - fw_dthr_weight[2]);
 			}
 
 			if (_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint)) {
@@ -427,10 +458,38 @@ ControlAllocator::Run()
 				c[1](4) = vehicle_thrust_setpoint.xyz[1];
 				c[1](5) = vehicle_thrust_setpoint.xyz[2];
 			}
+
+			// VTOL differential thrust in FW
+			if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT) {
+				/* CA_FW_DTHR_SC_R etc are scales to tune response of differential thrust around each axis.
+				The scaling factor is applied to the (normalized) torque setpoint from the rate controller going
+				to the motors for rate control in fixed-wing flight using differential thrust instead of
+				aerodynamic control surfaces.
+				Set this parameter such that when the control surfaces are disabled,
+				the systems rate tracking is maintained as best as possible through
+				differential thrust.
+				*/
+
+				c[0](0) = vehicle_torque_setpoint_matrix_1(0) * fw_dthr_scale[0] * fw_dthr_weight[0];
+				c[0](1) = vehicle_torque_setpoint_matrix_1(1) * fw_dthr_scale[1] * fw_dthr_weight[1];
+				c[0](2) = vehicle_torque_setpoint_matrix_1(2) * fw_dthr_scale[2] * fw_dthr_weight[2];
+
+				const bool has_non_zero_dthr_weight = fw_dthr_weight[0] > FLT_EPSILON || fw_dthr_weight[1] > FLT_EPSILON
+								      || fw_dthr_weight[2] > FLT_EPSILON;
+
+				if (has_non_zero_dthr_weight) {
+					_actuator_effectiveness->setEnableAuxiliaryMotors(true); // Currently only does something for Standard VTOL
+
+				} else {
+
+					_actuator_effectiveness->setEnableAuxiliaryMotors(false);
+				}
+			}
 		}
 
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
+			_control_allocation[i]->setAirmode(airmode);
 			_control_allocation[i]->setControlSetpoint(c[i]);
 
 			// Do allocation
@@ -541,15 +600,17 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
 			}
 		}
 
-		// Handle failed actuators
-		if (_handled_motor_failure_bitmask) {
+		const int16_t motor_failed_disabled_bitmask = _handled_motor_failure_bitmask | _handled_motor_disabled_bitmask;
+
+		// Handle failed/disabled motors
+		if (motor_failed_disabled_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)) {
+				if (motor_failed_disabled_bitmask & (1 << motors_idx)) {
 					ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix];
 
 					for (int i = 0; i < NUM_AXES; i++) {
@@ -562,6 +623,43 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
 			}
 		}
 
+		// Handle failed servos
+		if (_handled_servo_failure_bitmask) {
+			actuator_idx = 0;
+			memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix));
+
+			for (int servos_idx = 0; servos_idx < _num_actuators[0] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) {
+				const int selected_matrix = 1; // matrix 1 (only works for VTOL atm)
+
+				if (_handled_servo_failure_bitmask & (1 << servos_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.f;
+					}
+				}
+
+				++actuator_idx_matrix[selected_matrix];
+				++actuator_idx;
+			}
+		}
+
+		// handle servo failure injection
+		if (_handled_servo_center_mask) {
+
+			if (_handled_servo_center_mask & 1) {
+				// set the first servo to 0 by setting the min/max to 0 (center)
+				minimum[1](0) = 0.f;
+				maximum[1](0) = 0.f;
+			}
+
+			if (_handled_servo_center_mask & 2) {
+				// set the second servo to 0 by setting the min/max to 0 (center)
+				minimum[1](1) = 0.f;
+				maximum[1](1) = 0.f;
+			}
+		}
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 			_control_allocation[i]->setActuatorMin(minimum[i]);
 			_control_allocation[i]->setActuatorMax(maximum[i]);
@@ -584,6 +682,10 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
 
 				if (all_entries_small) {
 					matrix.row(n) = 0.f;
+					_has_control_authority[i][n] = false;
+
+				} else {
+					_has_control_authority[i][n] = true;
 				}
 			}
 
@@ -619,6 +721,13 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
 	control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
 	control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
 
+	control_allocator_status.torque_setpoint[0] = _control_allocation[matrix_index]->getControlSetpoint()(0);
+	control_allocator_status.torque_setpoint[1] = _control_allocation[matrix_index]->getControlSetpoint()(1);
+	control_allocator_status.torque_setpoint[2] = _control_allocation[matrix_index]->getControlSetpoint()(2);
+	control_allocator_status.thrust_setpoint[0] = _control_allocation[matrix_index]->getControlSetpoint()(3);
+	control_allocator_status.thrust_setpoint[1] = _control_allocation[matrix_index]->getControlSetpoint()(4);
+	control_allocator_status.thrust_setpoint[2] = _control_allocation[matrix_index]->getControlSetpoint()(5);
+
 	// override control_allocator_status in customized saturation logic for certain effectiveness types
 	_actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status);
 
@@ -670,7 +779,9 @@ ControlAllocator::publish_actuator_controls()
 	int actuator_idx = 0;
 	int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
 
-	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask;
+	// set output to NAN of motors that are stopped or disabled
+	const uint32_t stopped_or_disabled_motors = _actuator_effectiveness->getStoppedMotors() |
+			_actuator_effectiveness->getDisabledMotors();
 
 	// motors
 	int motors_idx;
@@ -680,7 +791,7 @@ ControlAllocator::publish_actuator_controls()
 		float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]);
 		actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN;
 
-		if (stopped_motors & (1u << motors_idx)) {
+		if (stopped_or_disabled_motors & (1u << motors_idx)) {
 			actuator_motors.control[motors_idx] = NAN;
 		}
 
@@ -714,13 +825,36 @@ ControlAllocator::publish_actuator_controls()
 	}
 }
 
-void
-ControlAllocator::check_for_motor_failures()
+bool
+ControlAllocator::check_for_actuator_activation_update()
 {
+	bool activation_updated = false;
+
 	failure_detector_status_s failure_detector_status;
 
+	const bool status_updated = _failure_detector_status_sub.update(&failure_detector_status);
+
+	// hack: set the servos in the failed bitmask to 0
+	if (status_updated) {
+		if (failure_detector_status.servo_to_center_mask) {
+			if (!_handled_servo_center_mask) {
+				PX4_WARN("Servo(s) to center failure injected");
+				_handled_servo_center_mask = failure_detector_status.servo_to_center_mask;
+				activation_updated = true;
+
+			}
+
+		} else {
+			if (_handled_servo_center_mask) {
+				PX4_INFO("Restoring servo(s)");
+				_handled_servo_center_mask = 0;
+				activation_updated = true;
+			}
+		}
+	}
+
 	if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE
-	    && _failure_detector_status_sub.update(&failure_detector_status)) {
+	    && status_updated) {
 		if (failure_detector_status.fd_motor) {
 
 			if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) {
@@ -734,12 +868,7 @@ ControlAllocator::check_for_motor_failures()
 						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);
+							activation_updated = true;
 						}
 					}
 					break;
@@ -755,13 +884,33 @@ ControlAllocator::check_for_motor_failures()
 			PX4_INFO("Restoring all motors");
 			_handled_motor_failure_bitmask = 0;
 
-			for (int i = 0; i < _num_control_allocation; ++i) {
-				_control_allocation[i]->setHadActuatorFailure(false);
+			activation_updated = true;
+		}
+
+		if (failure_detector_status.fd_servo) {
+			if (_handled_servo_failure_bitmask != failure_detector_status.servo_failure_mask) {
+				// servo failure bitmask changed
+				_handled_servo_failure_bitmask = failure_detector_status.servo_failure_mask;
+				PX4_WARN("Removing servo nr. %d from allocation", _handled_servo_failure_bitmask);
+				activation_updated = true;
 			}
 
-			update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE);
+		} else if (_handled_servo_failure_bitmask != 0) {
+			// Clear bitmask completely
+			PX4_INFO("Restoring all servos");
+			_handled_servo_failure_bitmask = 0;
+
+			activation_updated = true;
 		}
 	}
+
+	// handle disabled actuators (currently only VTOL motors)
+	if (_actuator_effectiveness->getDisabledMotors() != _handled_motor_disabled_bitmask) {
+		_handled_motor_disabled_bitmask = _actuator_effectiveness->getDisabledMotors();
+		activation_updated = true;
+	}
+
+	return activation_updated;
 }
 
 int ControlAllocator::task_spawn(int argc, char *argv[])
@@ -817,14 +966,35 @@ int ControlAllocator::print_status()
 
 	// Print current effectiveness matrix
 	for (int i = 0; i < _num_control_allocation; ++i) {
-		const ActuatorEffectiveness::EffectivenessMatrix &effectiveness = _control_allocation[i]->getEffectivenessMatrix();
+		const ActuatorEffectiveness::EffectivenessMatrix &effectiveness =
+			_control_allocation[i]->getEffectivenessMatrixAllocation();
 
 		if (_num_control_allocation > 1) {
 			PX4_INFO("Instance: %i", i);
 		}
 
-		PX4_INFO("  Effectiveness.T =");
-		effectiveness.T().print();
+		// const size_t num_actuators = _control_allocation[i]->numConfiguredActuators();
+		const size_t num_actuators = 6;
+
+		matrix::Matrix<float, num_actuators, NUM_AXES> effectiveness_sliced_transposed(
+			effectiveness.T().slice<num_actuators, NUM_AXES>(0, 0));
+
+		const matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> mix_raw = _control_allocation[i]->getMixMatrix();
+		const matrix::Matrix<float, num_actuators, NUM_AXES> mix_raw_sliced(mix_raw.slice<num_actuators, NUM_AXES>(0, 0));
+
+		const matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> mix_normalized = _control_allocation[i]->getNormalizedMixMatrix();
+		const matrix::Matrix<float, num_actuators, NUM_AXES> mix_normalized_sliced(
+			mix_normalized.slice<num_actuators, NUM_AXES>(0, 0));
+
+		PX4_INFO("  Effectiveness.T (sliced) =");
+		effectiveness_sliced_transposed.print();
+
+		PX4_INFO(" Mixer matrix raw (sliced) =");
+		mix_raw_sliced.print();
+
+		PX4_INFO(" Mixer matrix normalized (sliced) =");
+		mix_normalized_sliced.print();
+
 		PX4_INFO("  minimum =");
 		_control_allocation[i]->getActuatorMin().T().print();
 		PX4_INFO("  maximum =");

src/modules/control_allocator/ControlAllocator.hpp

@@ -132,7 +132,7 @@ private:
 
 	void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);
 
-	void check_for_motor_failures();
+	bool check_for_actuator_activation_update();
 
 	void publish_control_allocator_status(int matrix_index);
 
@@ -198,6 +198,10 @@ private:
 	// 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};
+	uint16_t _handled_servo_failure_bitmask{0};
+
+	uint16_t _handled_motor_disabled_bitmask{0};
+	uint16_t _handled_servo_center_mask{0};
 
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
@@ -209,12 +213,24 @@ private:
 	ParamHandles _param_handles{};
 	Params _params{};
 	bool _has_slew_rate{false};
+	ActuatorEffectiveness::FlightPhase _flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
+
+	bool _has_control_authority[2][6] = {false};
 
 	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
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
+		(ParamFloat<px4::params::CA_FW_DTHR_SC_R>) _param_ca_fw_dthr_sc_r,
+		(ParamFloat<px4::params::CA_FW_DTHR_SC_P>) _param_ca_fw_dthr_sc_p,
+		(ParamFloat<px4::params::CA_FW_DTHR_SC_Y>) _param_ca_fw_dthr_sc_y,
+		(ParamFloat<px4::params::CA_FW_DTHR_WGT_R>) _param_ca_fw_dthr_wgt_r,
+		(ParamFloat<px4::params::CA_FW_DTHR_WGT_P>) _param_ca_fw_dthr_wgt_p,
+		(ParamFloat<px4::params::CA_FW_DTHR_WGT_Y>) _param_ca_fw_dthr_wgt_y,
+		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
+		(ParamInt<px4::params::CA_FW_DTHR_AIRMD>) _param_ca_fw_dthr_airmd,
+		(ParamBool<px4::params::CA_FW_DTHR_FB_EN>) _param_ca_fw_dthr_fb_en
 	)
 
 };

src/modules/control_allocator/module.yaml

@@ -40,7 +40,7 @@ parameters:
                   If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
             type: enum
             values:
-                0: Pseudo-inverse with output clipping
+                0: Pseudo-inverse without any desaturation
                 1: Pseudo-inverse with sequential desaturation technique
                 2: Automatic
             default: 2
@@ -541,6 +541,98 @@ parameters:
                 1: Remove first failed motor from effectiveness
             default: 0
 
+        CA_FW_DTHR_SC_R:
+            description:
+                short: Fixed-wing differential thrust roll scale
+            type: float
+            decimal: 2
+            increment: 0.1
+            min: 0
+            max: 10
+            default: 1
+
+        CA_FW_DTHR_SC_P:
+            description:
+                short: Fixed-wing differential thrust pitch scale
+            type: float
+            decimal: 2
+            increment: 0.1
+            min: 0
+            max: 10
+            default: 1
+
+        CA_FW_DTHR_SC_Y:
+            description:
+                short: Fixed-wing differential thrust yaw scale
+            type: float
+            decimal: 2
+            increment: 0.1
+            min: 0
+            max: 10
+            default: 1
+
+        CA_FW_DTHR_WGT_R:
+            description:
+                short: Fixed-wing differential thrust roll weight
+                long: |
+                    Weight of torque allocated through differential thrust vs control surface.
+            type: float
+            decimal: 2
+            increment: 0.1
+            min: 0
+            max: 1
+            default: 0
+
+        CA_FW_DTHR_WGT_P:
+            description:
+                short: Pitch weight of differential thrust in fixed-wing
+                long: |
+                    Weight of torque allocated through differential thrust vs control surface.
+            type: float
+            decimal: 2
+            increment: 0.1
+            min: 0
+            max: 1
+            default: 0
+
+        CA_FW_DTHR_WGT_Y:
+            description:
+                short: Yaw weight of differential thrust in fixed-wing
+                long: |
+                    Weight of torque allocated through differential thrust vs control surface.
+            type: float
+            decimal: 2
+            increment: 0.1
+            min: 0
+            max: 1
+            default: 0
+
+        CA_FW_DTHR_AIRMD:
+            description:
+                short: Enable airmode for fixed-wing differential thrust
+                long: |
+                    If enabeld it allows the allocator to increase collective thrust to achieve
+                    thorque setpoints.
+            type: enum
+            values:
+                0: Disabled
+                1: Roll/Pitch
+                2: Roll/Pitch/Yaw
+            default: 0
+
+        CA_FW_DTHR_FB_EN:
+            description:
+                short: Enable fallback to differential thrust for control in fixed-wing
+                long: |
+                    Enable fallback to differential thrust for rate control around axes that have
+                    0 effectiveness in the servo matrix after the detected servo failure.
+            type: boolean
+            values:
+                0: Disabled
+                1: Enabled
+            default: 1
+
+
 # Mixer
 mixer:
     actuator_types:

src/modules/vtol_att_control/tailsitter.cpp

@@ -291,19 +291,6 @@ void Tailsitter::fill_actuator_outputs()
 
 		_thrust_setpoint_0->xyz[2] = -_vehicle_thrust_setpoint_virtual_fw->xyz[0];
 
-		/* allow differential thrust if enabled */
-		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
-			_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0] * _param_vt_fw_difthr_s_y.get();
-		}
-
-		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::PITCH_BIT)) {
-			_torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1] * _param_vt_fw_difthr_s_p.get();
-		}
-
-		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT)) {
-			_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_r.get();
-		}
-
 		// for the short period after switching to FW where there is no thrust published yet from the FW controller,
 		// keep publishing the last MC thrust to keep the motors running
 		if (hrt_elapsed_time(&_trans_finished_ts) < 50_ms) {

src/modules/vtol_att_control/tiltrotor.cpp

@@ -379,10 +379,6 @@ void Tiltrotor::fill_actuator_outputs()
 		collective_thrust_normalized_setpoint = _vehicle_thrust_setpoint_virtual_fw->xyz[0];
 		_thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint;
 
-		/* allow differential thrust if enabled */
-		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
-			_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get() ;
-		}
 
 	} else {
 		collective_thrust_normalized_setpoint = -_vehicle_thrust_setpoint_virtual_mc->xyz[2] * _mc_throttle_weight;

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -283,61 +283,6 @@ PARAM_DEFINE_INT32(VT_FW_QC_HMAX, 0);
  */
 PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
 
-/**
- * Differential thrust in forwards flight.
- *
- * Enable differential thrust seperately for roll, pitch, yaw in forward (fixed-wing) mode.
- * The effectiveness of differential thrust around the corresponding axis can be
- * tuned by setting VT_FW_DIFTHR_S_R / VT_FW_DIFTHR_S_P / VT_FW_DIFTHR_S_Y.
- *
- * @min 0
- * @max 7
- * @bit 0 Yaw
- * @bit 1 Roll
- * @bit 2 Pitch
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
-
-/**
- * Roll differential thrust factor in forward flight
- *
- * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
- *
- * @min 0.0
- * @max 2.0
- * @decimal 2
- * @increment 0.1
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_R, 1.f);
-
-/**
- * Pitch differential thrust factor in forward flight
- *
- * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
- *
- * @min 0.0
- * @max 2.0
- * @decimal 2
- * @increment 0.1
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f);
-
-/**
- * Yaw differential thrust factor in forward flight
- *
- * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
- *
- * @min 0.0
- * @max 2.0
- * @decimal 2
- * @increment 0.1
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f);
-
 /**
  * Backtransition deceleration setpoint to pitch I gain.
  *

src/modules/vtol_att_control/vtol_type.h

@@ -76,12 +76,6 @@ enum VtolForwardActuationMode {
 	ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND
 };
 
-// enum for bitmask of VT_FW_DIFTHR_EN parameter options
-enum class VtFwDifthrEnBits : int32_t {
-	YAW_BIT = (1 << 0),
-	ROLL_BIT = (1 << 1),
-	PITCH_BIT = (1 << 2),
-};
 
 enum class QuadchuteReason {
 	None = 0,
@@ -344,10 +338,6 @@ protected:
 					(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
 					(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout,
 					(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
-					(ParamInt<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
-					(ParamFloat<px4::params::VT_FW_DIFTHR_S_Y>) _param_vt_fw_difthr_s_y,
-					(ParamFloat<px4::params::VT_FW_DIFTHR_S_P>) _param_vt_fw_difthr_s_p,
-					(ParamFloat<px4::params::VT_FW_DIFTHR_S_R>) _param_vt_fw_difthr_s_r,
 					(ParamFloat<px4::params::VT_B_DEC_I>) _param_vt_b_dec_i,
 					(ParamFloat<px4::params::VT_B_DEC_MSS>) _param_vt_b_dec_mss,