refactor(control_allocator): simplify motor direction classification & handling

Functional change: The directions are now handled uniformly, and motors
pointing along a given axis _in both directions_ are classified as
pointing along that axis. The names are changed accordingly:
 - longitudinal = forward and backward
 - lateral = left and right
 - vertical = up and down

Cosmetic / maintainability changes:
 - rather than repeating the function for each direction, the same code
   now handles all directions
 - the bitmasks are now in a struct, so we can pass them around together,
   avoiding the need for every single effectivenss class to manually set
   the three masks.
 - reorder in some places to always match x-y-z

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -95,15 +95,15 @@ ActuatorEffectiveness::ActuatorBitmask ActuatorEffectiveness::getStoppedMotors()
 	// additionally be stopped in the ControlAllocator, due to motor
 	// failure.
 
-	//  a) they are turned off because they are generally not used in the given flight phase.
+	//  a) because they are generally not used in the given flight phase.
 	ActuatorEffectiveness::ActuatorBitmask stopped_motors_mask = _stopped_motors_mask_due_to_flight_phase;
 
-	//  b) they are stopped because the thrust setpoint in a given direction is NaN
-	if (_forwards_motors_stopped_by_thrust) { stopped_motors_mask |= _forwards_motors_mask; }
+	//  b) because the thrust setpoint in a given direction is NaN
+	if (_longitudinal_motors_stopped_by_thrust) { stopped_motors_mask |= _motor_direction_bitmasks.longitudinal; }
 
-	if (_upwards_motors_stopped_by_thrust) { stopped_motors_mask |= _upwards_motors_mask; }
+	if (_lateral_motors_stopped_by_thrust) { stopped_motors_mask |= _motor_direction_bitmasks.lateral; }
 
-	if (_sideways_motors_stopped_by_thrust) { stopped_motors_mask |= _sideways_motors_mask; }
+	if (_vertical_motors_stopped_by_thrust) { stopped_motors_mask |= _motor_direction_bitmasks.vertical; }
 
 	return stopped_motors_mask;
 }

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -220,9 +220,9 @@ public:
 	 */
 	void stopMotorsBasedOnThrustSetpoint(const matrix::Vector3f &thrust_sp)
 	{
-		_forwards_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(0));
-		_sideways_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(1));
-		_upwards_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(2));
+		_longitudinal_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(0));
+		_lateral_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(1));
+		_vertical_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(2));
 	}
 
 protected:
@@ -231,15 +231,17 @@ protected:
 	// ActuatorEffectivenessRotors::get{Upwards,Forwards,Sideways}Motors()
 	// They can alternatively be set to zero to completely disable stopping motors based on NaN thrust
 
-	ActuatorBitmask _upwards_motors_mask{};
-	ActuatorBitmask _forwards_motors_mask{};
-	ActuatorBitmask _sideways_motors_mask{};
+	struct MotorDirectionBitmasks {
+		ActuatorBitmask longitudinal{};
+		ActuatorBitmask lateral{};
+		ActuatorBitmask vertical{};
+	} _motor_direction_bitmasks;
 
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
 	ActuatorBitmask _stopped_motors_mask_due_to_flight_phase{};
 
-	bool _forwards_motors_stopped_by_thrust{false};
-	bool _upwards_motors_stopped_by_thrust{false};
-	bool _sideways_motors_stopped_by_thrust{false};
+	bool _longitudinal_motors_stopped_by_thrust{false};
+	bool _vertical_motors_stopped_by_thrust{false};
+	bool _lateral_motors_stopped_by_thrust{false};
 
 };

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -52,7 +52,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	// Motors
 	_rotors.enablePropellerTorque(false);
 	const bool rotors_added_successfully = _rotors.addActuators(configuration);
-	_forwards_motors_mask = _rotors.getForwardsMotors();
+	_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks);
 
 	// Control Surfaces
 	_first_control_surface_idx = configuration.num_actuators_matrix[0];

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

@@ -59,9 +59,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 	_tilts.updateTorqueSign(_mc_rotors.geometry());
 	const bool tilts_added_successfully = _tilts.addActuators(configuration);
 
-	_forwards_motors_mask = _mc_rotors.getForwardsMotors();
-	_sideways_motors_mask = _mc_rotors.getSidewaysMotors();
-	_upwards_motors_mask = _mc_rotors.getUpwardsMotors();
+	_mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks);
 
 	// Set offset such that tilts point upwards when control input == 0 (trim is 0 if min_angle == -max_angle).
 	// Note that we don't set configuration.trim here, because in the case of trim == +-1, yaw is always saturated

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -51,10 +51,7 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(Configuration &configura
 
 	// Motors
 	const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
-
-	_forwards_motors_mask = _mc_rotors.getForwardsMotors();
-	_sideways_motors_mask = _mc_rotors.getSidewaysMotors();
-	_upwards_motors_mask = _mc_rotors.getUpwardsMotors();
+	_mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks);
 
 	return rotors_added_successfully;
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -43,8 +43,6 @@
 
 #include "ActuatorEffectivenessTilts.hpp"
 
-using namespace matrix;
-
 ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config,
 		bool tilt_support)
 	: ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support)
@@ -267,55 +265,38 @@ ActuatorBitmask ActuatorEffectivenessRotors::getMotors() const
 	return motors;
 }
 
-// These three allow an angle deviation of 8.1 deg. Motor tilt for yaw actuation
-// could reasonably be more. Alternatively, we could classify the motors such
-// that every rotor points in a defined direction, putting the decision
-// boundaries at 45 deg. TODO revisit
-
-ActuatorBitmask ActuatorEffectivenessRotors::getUpwardsMotors() const
+// Helper to check if a vector is primarily aligned with a specific axis index
+bool ActuatorEffectivenessRotors::isAlignedWithAxis(const Vector3f &axis_abs, int primary_idx)
 {
-	ActuatorBitmask upwards_motors = 0;
+	for (int i = 0; i < 3; ++i) {
+		if (i == primary_idx) {
+			if (axis_abs(i) <= 0.1f) { return false; }
 
-	for (int i = 0; i < _geometry.num_rotors; ++i) {
-		const Vector3f &axis = _geometry.rotors[i].axis;
-
-		if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) {
-			upwards_motors |= 1u << i;
+		} else {
+			if (axis_abs(i) >= 0.5f) { return false; }
 		}
 	}
 
-	return upwards_motors;
+	return true;
 }
 
-ActuatorBitmask ActuatorEffectivenessRotors::getForwardsMotors() const
+void ActuatorEffectivenessRotors::setMotorDirectionBitmasks(ActuatorEffectiveness::MotorDirectionBitmasks &masks)
 {
-	ActuatorBitmask forward_motors = 0;
+	masks.longitudinal = masks.lateral = masks.vertical = 0;
 
 	for (int i = 0; i < _geometry.num_rotors; ++i) {
-		const Vector3f &axis = _geometry.rotors[i].axis;
+		const Vector3f &axis_abs = _geometry.rotors[i].axis.abs();
 
-		if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) {
-			forward_motors |= 1u << i;
+		if (isAlignedWithAxis(axis_abs, 0)) { // X-axis
+			masks.longitudinal |= 1 << i;
+
+		} else if (isAlignedWithAxis(axis_abs, 1)) { // Y-axis
+			masks.lateral |= 1 << i;
+
+		} else if (isAlignedWithAxis(axis_abs, 2)) { // Z-axis
+			masks.vertical |= 1 << i;
 		}
 	}
-
-	return forward_motors;
-}
-
-ActuatorBitmask ActuatorEffectivenessRotors::getSidewaysMotors() const
-{
-	ActuatorBitmask sideways_motors = 0;
-
-	for (int i = 0; i < _geometry.num_rotors; ++i) {
-		const Vector3f &axis = _geometry.rotors[i].axis;
-
-		// This includes both left and right pointing motors, in contrast to the other two directions...
-		if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) > 0.5f && fabsf(axis(2)) < 0.1f) {
-			sideways_motors |= 1u << i;
-		}
-	}
-
-	return sideways_motors;
 }
 
 bool

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -50,9 +50,13 @@
 class ActuatorEffectivenessTilts;
 
 using namespace time_literals;
+using namespace matrix;
 
 using ActuatorBitmask = ActuatorEffectiveness::ActuatorBitmask;
 
+static constexpr float MIN_AXIS_DOMINANT = 0.5f;
+static constexpr float MAX_AXIS_NEGLIGIBLE = 0.5f;
+
 class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness
 {
 public:
@@ -130,15 +134,15 @@ public:
 
 	ActuatorBitmask getMotors() const;
 
-	ActuatorBitmask getUpwardsMotors() const;
-	ActuatorBitmask getForwardsMotors() const;
-	ActuatorBitmask getSidewaysMotors() const;
+	void setMotorDirectionBitmasks(ActuatorEffectiveness::MotorDirectionBitmasks &masks);
 
 private:
 	void updateParams() override;
 	const AxisConfiguration _axis_config;
 	const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
 
+	bool isAlignedWithAxis(const Vector3f &axis_abs, int primary_idx);
+
 	struct ParamHandles {
 		param_t position_x;
 		param_t position_y;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -52,8 +52,7 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
 	configuration.selected_matrix = 0;
 	_rotors.enablePropellerTorqueNonUpwards(false);
 	const bool mc_rotors_added_successfully = _rotors.addActuators(configuration);
-	_upwards_motors_mask = _rotors.getUpwardsMotors();
-	_forwards_motors_mask = _rotors.getForwardsMotors();
+	_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks);
 
 	// Control Surfaces
 	configuration.selected_matrix = 1;
@@ -93,7 +92,7 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight
 
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors_mask_due_to_flight_phase = _upwards_motors_mask;
+		_stopped_motors_mask_due_to_flight_phase = _motor_direction_bitmasks.vertical;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -106,19 +106,22 @@ void ActuatorEffectivenessTailsitterVTOL::updateMotorMasks()
 	switch (_flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
 
-		// TODO only set this mask if the motors are not needed for rate control, watching one or both of:
-		//  - _mc_rotors.geometry().num_rotors > 3 (used in getEffectivenessMatrix to enable MC yaw control)
-		//  - VT_FW_DIFTHR_EN (the general switch for this -- are the two equivalent?)
+		_mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks);
+
+		// Exchange forwards and upwards bitmasks - allocation frame
+		// upwards direction now points forwards
+		{
+			const ActuatorBitmask tmp = _motor_direction_bitmasks.longitudinal;
+			_motor_direction_bitmasks.longitudinal = _motor_direction_bitmasks.vertical;
+			_motor_direction_bitmasks.vertical = tmp;
+		}
 
-		_forwards_motors_mask = _mc_rotors.getUpwardsMotors(); // allocation frame they stay upwards
-		_upwards_motors_mask = 0;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
-		_forwards_motors_mask = 0;
-		_upwards_motors_mask = _mc_rotors.getUpwardsMotors();
+		_mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks);
 
 		break;
 	}

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -83,8 +83,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 			     << configuration.num_actuators[(int)ActuatorType::MOTORS];
 
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
-	_forwards_motors_mask = _mc_rotors.getForwardsMotors();
-	_upwards_motors_mask = _mc_rotors.getUpwardsMotors();
+	_mc_rotors.setMotorDirectionBitmasks(_motor_direction_bitmasks);
 	_motors = _mc_rotors.getMotors();
 
 	// Control Surfaces