control_allocator: change SequentialDesaturation to existing MC mixer

And limit the operations to the number of configured outputs.

Only using the number of configured actuators reduces CPU load by ~2% on
F7 @1khz.

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -99,6 +99,11 @@ public:
 		return _flight_phase;
 	}
 
+	/**
+	 * Get the number of actuators
+	 */
+	virtual int numActuators() const = 0;
+
 protected:
 	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -132,17 +132,19 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NU
 		geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
 		geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
 
-		computeEffectivenessMatrix(geometry, matrix);
+		_num_actuators = computeEffectivenessMatrix(geometry, matrix);
 		return true;
 	}
 
 	return false;
 }
 
-void
+int
 ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry,
 		matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness)
 {
+	int num_actuators = 0;
+
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
 		// Get rotor axis
 		matrix::Vector3f axis(
@@ -173,6 +175,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		float ct = geometry.rotors[i].thrust_coef;
 		float km = geometry.rotors[i].moment_ratio;
 
+		if (fabsf(ct) < FLT_EPSILON) {
+			continue;
+		}
+
 		// Compute thrust generated by this rotor
 		matrix::Vector3f thrust = ct * axis;
 
@@ -184,5 +190,9 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 			effectiveness(j, i) = moment(j);
 			effectiveness(j + 3, i) = thrust(j);
 		}
+
+		num_actuators = i + 1;
 	}
+
+	return num_actuators;
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -70,16 +70,18 @@ public:
 		RotorGeometry rotors[NUM_ROTORS_MAX];
 	} MultirotorGeometry;
 
-	static void computeEffectivenessMatrix(const MultirotorGeometry &geometry,
-					       matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
+	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
+					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
 
 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
 
+	int numActuators() const override { return _num_actuators; }
 private:
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< parameter updates subscription */
 
 	bool _updated{true};
+	int _num_actuators{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -58,6 +58,7 @@ public:
 	 */
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
+	int numActuators() const override { return 7; }
 protected:
 	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -58,6 +58,7 @@ public:
 	 */
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
+	int numActuators() const override { return 10; }
 protected:
 	bool _updated{true};
 };