refactor control_allocator: directly get the effectiveness matrix when updated

Reduces stack + RAM usage

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -62,13 +62,6 @@ public:
 		TRANSITION_FF_TO_HF = 3
 	};
 
-	/**
-	 * Update effectiveness matrix
-	 *
-	 * @return True if the effectiveness matrix has changed
-	 */
-	virtual bool update() = 0;
-
 	/**
 	 * Set the current flight phase
 	 *
@@ -77,17 +70,14 @@ public:
 	virtual void setFlightPhase(const FlightPhase &flight_phase)
 	{
 		_flight_phase = flight_phase;
-	};
+	}
 
 	/**
-	 * Get the control effectiveness matrix
+	 * Get the control effectiveness matrix if updated
 	 *
-	 * @return Effectiveness matrix
+	 * @return true if updated and matrix is set
 	 */
-	const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const
-	{
-		return _effectiveness;
-	};
+	virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) = 0;
 
 	/**
 	 * Get the actuator trims
@@ -97,7 +87,7 @@ public:
 	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
 	{
 		return _trim;
-	};
+	}
 
 	/**
 	 * Get the current flight phase
@@ -107,10 +97,9 @@ public:
 	const FlightPhase &getFlightPhase() const
 	{
 		return _flight_phase;
-	};
+	}
 
 protected:
-	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  //< Effectiveness matrix
-	matrix::Vector<float, NUM_ACTUATORS> _trim;			//< Actuator trim
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		//< Current flight phase
+	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -44,35 +44,105 @@
 ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor():
 	ModuleParams(nullptr)
 {
-	parameters_updated();
 }
 
 bool
-ActuatorEffectivenessMultirotor::update()
+ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
 {
-	bool updated = false;
-
 	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
+	if (_updated || _parameter_update_sub.updated()) {
 		// clear update
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
 
 		updateParams();
-		parameters_updated();
+		_updated = false;
 
-		updated = true;
+		// Get multirotor geometry
+		MultirotorGeometry geometry = {};
+		geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
+		geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
+		geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
+		geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
+		geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
+		geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
+		geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
+		geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
+
+		geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
+		geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
+		geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
+		geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
+		geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
+		geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
+		geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
+		geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
+
+		geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
+		geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
+		geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
+		geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
+		geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
+		geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
+		geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
+		geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
+
+		geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
+		geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
+		geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
+		geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
+		geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
+		geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
+		geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
+		geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
+
+		geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
+		geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
+		geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
+		geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
+		geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
+		geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
+		geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
+		geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
+
+		geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
+		geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
+		geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
+		geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
+		geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
+		geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
+		geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
+		geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
+
+		geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
+		geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
+		geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
+		geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
+		geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
+		geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
+		geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
+		geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
+
+		geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
+		geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
+		geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
+		geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
+		geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
+		geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
+		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);
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
-matrix::Matrix<float, ActuatorEffectivenessMultirotor::NUM_AXES, ActuatorEffectivenessMultirotor::NUM_ACTUATORS>
-ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry geometry)
+void
+ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry,
+		matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness)
 {
-	matrix::Matrix<float, ActuatorEffectivenessMultirotor::NUM_AXES, ActuatorEffectivenessMultirotor::NUM_ACTUATORS>
-	effectiveness;
-
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
 		// Get rotor axis
 		matrix::Vector3f axis(
@@ -115,87 +185,4 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry g
 			effectiveness(j + 3, i) = thrust(j);
 		}
 	}
-
-	return effectiveness;
-}
-
-void
-ActuatorEffectivenessMultirotor::parameters_updated()
-{
-	// Get multirotor geometry
-	MultirotorGeometry geometry = {};
-	geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
-	geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
-	geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
-	geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
-	geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
-	geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
-	geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
-	geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
-
-	geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
-	geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
-	geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
-	geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
-	geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
-	geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
-	geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
-	geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
-
-	geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
-	geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
-	geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
-	geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
-	geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
-	geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
-	geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
-	geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
-
-	geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
-	geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
-	geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
-	geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
-	geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
-	geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
-	geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
-	geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
-
-	geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
-	geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
-	geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
-	geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
-	geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
-	geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
-	geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
-	geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
-
-	geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
-	geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
-	geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
-	geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
-	geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
-	geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
-	geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
-	geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
-
-	geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
-	geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
-	geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
-	geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
-	geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
-	geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
-	geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
-	geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
-
-	geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
-	geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
-	geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
-	geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
-	geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
-	geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
-	geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
-	geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
-
-	// Compute effectiveness matrix
-	_effectiveness = computeEffectivenessMatrix(geometry);
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -53,13 +53,6 @@ public:
 	ActuatorEffectivenessMultirotor();
 	virtual ~ActuatorEffectivenessMultirotor() = default;
 
-	/**
-	 * Update effectiveness matrix
-	 *
-	 * @return True if the effectiveness matrix has changed
-	 */
-	virtual bool update() override;
-
 	static constexpr int NUM_ROTORS_MAX = 8;
 
 	typedef struct {
@@ -77,16 +70,17 @@ public:
 		RotorGeometry rotors[NUM_ROTORS_MAX];
 	} MultirotorGeometry;
 
-	static matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> computeEffectivenessMatrix(MultirotorGeometry);
+	static void computeEffectivenessMatrix(const MultirotorGeometry &geometry,
+					       matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
+
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
 
 private:
-	/**
-	 * initialize some vectors/matrices from parameters
-	 */
-	void parameters_updated();
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< parameter updates subscription */
 
+	bool _updated{true};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,
 		(ParamFloat<px4::params::CA_MC_R0_PY>) _param_ca_mc_r0_py,

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -47,23 +47,12 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
 }
 
 bool
-ActuatorEffectivenessStandardVTOL::update()
+ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
 {
-	if (_updated) {
-		_updated = false;
-		return true;
+	if (!_updated) {
+		return false;
 	}
 
-	return false;
-}
-
-void
-ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
-{
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-
-	_updated = true;
-
 	switch (_flight_phase) {
 	case FlightPhase::HOVER_FLIGHT:  {
 			const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
@@ -74,7 +63,7 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas
 				{ 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
 				{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
 			};
-			_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
+			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
 			break;
 		}
 
@@ -87,7 +76,7 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas
 				{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
 				{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
 			};
-			_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
+			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
 			break;
 		}
 
@@ -101,8 +90,19 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas
 				{  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
 				{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
 			};
-			_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
+			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
 			break;
 		}
 	}
+
+	_updated = false;
+	return true;
+}
+
+void
+ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+	_updated = true;
+
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -49,12 +49,7 @@ public:
 	ActuatorEffectivenessStandardVTOL();
 	virtual ~ActuatorEffectivenessStandardVTOL() = default;
 
-	/**
-	 * Update effectiveness matrix
-	 *
-	 * @return True if the effectiveness matrix has changed
-	 */
-	virtual bool update() override;
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
 
 	/**
 	 * Set the current flight phase
@@ -64,5 +59,5 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 protected:
-	bool _updated{false};
+	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -45,25 +45,13 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
 {
 	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
-
 bool
-ActuatorEffectivenessTiltrotorVTOL::update()
+ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
 {
-	if (_updated) {
-		_updated = false;
-		return true;
+	if (!_updated) {
+		return false;
 	}
 
-	return false;
-}
-
-void
-ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
-{
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-
-	_updated = true;
-
 	// Trim
 	float tilt = 0.0f;
 
@@ -104,13 +92,24 @@ ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_pha
 		{ 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
 		{-0.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
 	};
-	_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(tiltrotor_vtol);
+	matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(tiltrotor_vtol);
 
 	// Temporarily disable a few controls (WIP)
 	for (size_t j = 4; j < 8; j++) {
-		_effectiveness(3, j) = 0.0f;
-		_effectiveness(4, j) = 0.0f;
-		_effectiveness(5, j) = 0.0f;
+		matrix(3, j) = 0.0f;
+		matrix(4, j) = 0.0f;
+		matrix(5, j) = 0.0f;
 	}
 
+
+	_updated = false;
+	return true;
+}
+
+void
+ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+
+	_updated = true;
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -49,12 +49,7 @@ public:
 	ActuatorEffectivenessTiltrotorVTOL();
 	virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
 
-	/**
-	 * Update effectiveness matrix
-	 *
-	 * @return True if the effectiveness matrix has changed
-	 */
-	virtual bool update() override;
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
 
 	/**
 	 * Set the current flight phase
@@ -64,5 +59,5 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 protected:
-	bool _updated{false};
+	bool _updated{true};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -132,13 +132,6 @@ ControlAllocator::parameters_updated()
 	actuator_max(14) = _param_ca_act14_max.get();
 	actuator_max(15) = _param_ca_act15_max.get();
 	_control_allocation->setActuatorMax(actuator_max);
-
-	// Get actuator effectiveness and trim
-	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness = _actuator_effectiveness->getEffectivenessMatrix();
-	matrix::Vector<float, NUM_ACTUATORS> trim = _actuator_effectiveness->getActuatorTrim();
-
-	// Assign control effectiveness matrix
-	_control_allocation->setEffectivenessMatrix(effectiveness, trim);
 }
 
 void
@@ -323,30 +316,7 @@ ControlAllocator::Run()
 	if (do_update) {
 		_last_run = now;
 
-		// Update effectiveness matrix if needed
-		if (_actuator_effectiveness->update()) {
-			matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness = _actuator_effectiveness->getEffectivenessMatrix();
-			matrix::Vector<float, NUM_ACTUATORS> trim = _actuator_effectiveness->getActuatorTrim();
-
-			// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)
-			matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
-			matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
-
-			for (size_t j = 0; j < NUM_ACTUATORS; j++) {
-				if (actuator_min(j) >= actuator_max(j)) {
-					for (size_t i = 0; i < NUM_AXES; i++) {
-						effectiveness(i, j) = 0.0f;
-					}
-
-				}
-			}
-
-			// Assign control effectiveness matrix
-			if ((effectiveness - _control_allocation->getEffectivenessMatrix()).abs().max() > 1e-5f) {
-				_control_allocation->setEffectivenessMatrix(effectiveness, trim);
-			}
-
-		}
+		update_effectiveness_matrix_if_needed();
 
 		// Set control setpoint vector
 		matrix::Vector<float, NUM_AXES> c;
@@ -374,6 +344,31 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
+void
+ControlAllocator::update_effectiveness_matrix_if_needed()
+{
+	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness;
+
+	if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) {
+		const matrix::Vector<float, NUM_ACTUATORS> &trim = _actuator_effectiveness->getActuatorTrim();
+
+		// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)
+		matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
+		matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
+
+		for (size_t j = 0; j < NUM_ACTUATORS; j++) {
+			if (actuator_min(j) >= actuator_max(j)) {
+				for (size_t i = 0; i < NUM_AXES; i++) {
+					effectiveness(i, j) = 0.0f;
+				}
+			}
+		}
+
+		// Assign control effectiveness matrix
+		_control_allocation->setEffectivenessMatrix(effectiveness, trim, _actuator_effectiveness->numActuators());
+	}
+}
+
 void
 ControlAllocator::publish_actuator_setpoint()
 {

src/modules/control_allocator/ControlAllocator.hpp

@@ -105,6 +105,8 @@ private:
 	void update_allocation_method();
 	void update_effectiveness_source();
 
+	void update_effectiveness_matrix_if_needed();
+
 	void publish_actuator_setpoint();
 	void publish_control_allocator_status();