CA: refactor logic for matrix updating

-pass flag EffectivenessUpdateReason into effectiveness, indicating if there was an external
update or not. Reasons for external updates are:
  -config changes (parameter)
  -motor failure detected or certain redundant motors are switched off to save energy

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

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -57,6 +57,12 @@ enum class ActuatorType {
 	COUNT
 };
 
+enum class EffectivenessUpdateReason {
+	NO_EXTERNAL_UPDATE = 0,
+	CONFIGURATION_UPDATE = 1,
+	MOTOR_ACTIVATION_UPDATE = 2,
+};
+
 
 class ActuatorEffectiveness
 {
@@ -147,7 +153,7 @@ public:
 	 *
 	 * @return true if updated and matrix is set
 	 */
-	virtual bool getEffectivenessMatrix(Configuration &configuration, bool force) = 0;
+	virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;}
 
 	/**
 	 * Get the current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -124,12 +124,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 	}
 }
 
-bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration &configuration, bool force)
+bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configuration)
 {
-	if (!force) {
-		return false;
-	}
-
 	for (int i = 0; i < _count; i++) {
 		int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp

@@ -69,7 +69,7 @@ public:
 	ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessControlSurfaces() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool addActuators(Configuration &configuration);
 
 	const char *name() const override { return "Control Surfaces"; }
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -41,19 +41,20 @@ ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent)
 }
 
 bool
-ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
 {
-	if (!force) {
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
 	// motors
 	_motors.enableYawControl(false);
-	_motors.getEffectivenessMatrix(configuration, true);
+	const bool motors_added_successfully = _motors.addActuators(configuration);
 
 	// Torque
-	_torque.getEffectivenessMatrix(configuration, true);
+	const bool torque_added_successfully = _torque.addActuators(configuration);
 
-	return true;
+	return (motors_added_successfully && torque_added_successfully);
 }
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -43,7 +43,7 @@ public:
 	ActuatorEffectivenessCustom(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessCustom() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
 	const char *name() const override { return "Custom"; }
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -43,21 +43,22 @@ ActuatorEffectivenessFixedWing::ActuatorEffectivenessFixedWing(ModuleParams *par
 }
 
 bool
-ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
 {
-	if (!force) {
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
 	// Motors
 	_rotors.enableYawControl(false);
-	_rotors.getEffectivenessMatrix(configuration, true);
+	const bool rotors_added_successfully = _rotors.addActuators(configuration);
 
 	// Control Surfaces
 	_first_control_surface_idx = configuration.num_actuators_matrix[0];
-	_control_surfaces.getEffectivenessMatrix(configuration, true);
+	const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
 
-	return true;
+	return (rotors_added_successfully && surfaces_added_successfully);
 }
 
 void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -45,7 +45,7 @@ public:
 	ActuatorEffectivenessFixedWing(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessFixedWing() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
 	const char *name() const override { return "Fixed Wing"; }
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

@@ -43,20 +43,21 @@ ActuatorEffectivenessMCTilt::ActuatorEffectivenessMCTilt(ModuleParams *parent)
 }
 
 bool
-ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
 {
-	if (!force) {
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
 	// MC motors
 	_mc_rotors.enableYawControl(!_tilts.hasYawControl());
-	_mc_rotors.getEffectivenessMatrix(configuration, true);
+	const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
 
 	// Tilts
 	int first_tilt_idx = configuration.num_actuators_matrix[0];
 	_tilts.updateTorqueSign(_mc_rotors.geometry());
-	_tilts.getEffectivenessMatrix(configuration, true);
+	const bool tilts_added_successfully = _tilts.addActuators(configuration);
 
 	// 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
@@ -72,7 +73,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 		}
 	}
 
-	return true;
+	return (rotors_added_successfully && tilts_added_successfully);
 }
 
 void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -43,7 +43,7 @@ public:
 	ActuatorEffectivenessMCTilt(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessMCTilt() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
 	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
 	{

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ActuatorEffectivenessMultirotor.hpp"
+
+using namespace matrix;
+
+ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent)
+	: ModuleParams(parent),
+	  _mc_rotors(this)
+{
+}
+
+bool
+ActuatorEffectivenessMultirotor::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	// Motors
+	const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
+
+	return rotors_added_successfully;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+#include "ActuatorEffectivenessRotors.hpp"
+
+class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	ActuatorEffectivenessMultirotor(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessMultirotor() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
+	}
+
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+	}
+
+	const char *name() const override { return "Multirotor"; }
+
+protected:
+	ActuatorEffectivenessRotors _mc_rotors;
+};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -136,24 +136,18 @@ void ActuatorEffectivenessRotors::updateParams()
 }
 
 bool
-ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessRotors::addActuators(Configuration &configuration)
 {
-	if (_updated || force) {
+	if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
-		_updated = false;
+		PX4_ERR("Wrong actuator ordering: servos need to be after motors");
-
+		return false;
-		if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
-			PX4_ERR("Wrong actuator ordering: servos need to be after motors");
-			return false;
-		}
-
-		int num_actuators = computeEffectivenessMatrix(_geometry,
-				    configuration.effectiveness_matrices[configuration.selected_matrix],
-				    configuration.num_actuators_matrix[configuration.selected_matrix]);
-		configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
-		return true;
 	}
 
-	return false;
+	int num_actuators = computeEffectivenessMatrix(_geometry,
+			    configuration.effectiveness_matrices[configuration.selected_matrix],
+			    configuration.num_actuators_matrix[configuration.selected_matrix]);
+	configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
+	return true;
 }
 
 int

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -93,7 +93,7 @@ public:
 	static int computeEffectivenessMatrix(const Geometry &geometry,
 					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool addActuators(Configuration &configuration);
 
 	const char *name() const override { return "Multirotor"; }
 
@@ -117,8 +117,6 @@ public:
 
 private:
 	void updateParams() override;
-
-	bool _updated{true};
 	const AxisConfiguration _axis_config;
 	const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp

@@ -37,9 +37,10 @@
 using namespace matrix;
 
 bool
-ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
 {
-	if (!force) {
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp

@@ -41,7 +41,7 @@ public:
 	ActuatorEffectivenessRoverAckermann() = default;
 	virtual ~ActuatorEffectivenessRoverAckermann() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
 	const char *name() const override { return "Rover (Ackermann)"; }
 private:

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp

@@ -37,9 +37,10 @@
 using namespace matrix;
 
 bool
-ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
 {
-	if (!force) {
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp

@@ -41,7 +41,7 @@ public:
 	ActuatorEffectivenessRoverDifferential() = default;
 	virtual ~ActuatorEffectivenessRoverDifferential() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
 	const char *name() const override { return "Rover (Differential)"; }
 private:

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -42,15 +42,16 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParam
 }
 
 bool
-ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
 {
-	if (!force) {
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
 	// MC motors
 	configuration.selected_matrix = 0;
-	_mc_rotors.getEffectivenessMatrix(configuration, true);
+	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
 	_mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1;
 
 	// Pusher/Puller
@@ -63,9 +64,9 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
 	// Control Surfaces
 	configuration.selected_matrix = 1;
 	_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
-	_control_surfaces.getEffectivenessMatrix(configuration, true);
+	const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
 
-	return true;
+	return (mc_rotors_added_successfully && surfaces_added_successfully);
 }
 
 void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -53,7 +53,7 @@ public:
 	ActuatorEffectivenessStandardVTOL(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessStandardVTOL() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
 	const char *name() const override { return "Standard VTOL"; }
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -47,24 +47,24 @@ ActuatorEffectivenessTailsitterVTOL::ActuatorEffectivenessTailsitterVTOL(ModuleP
 	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
 bool
-ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
 {
-	if (!force) {
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
 	// MC motors
 	configuration.selected_matrix = 0;
 	_mc_rotors.enableYawControl(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors
-
+	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
-	_mc_rotors.getEffectivenessMatrix(configuration, true);
 
 	// Control Surfaces
 	configuration.selected_matrix = 1;
 	_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
-	_control_surfaces.getEffectivenessMatrix(configuration, true);
+	const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
 
-	return true;
+	return (mc_rotors_added_successfully && surfaces_added_successfully);
 }
 
 void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -52,7 +52,7 @@ public:
 	ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessTailsitterVTOL() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
 	int numMatrices() const override { return 2; }
 
@@ -75,7 +75,6 @@ public:
 
 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
 protected:
-	bool _updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -51,9 +51,10 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModulePar
 	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
 bool
-ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
+ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
 {
-	if (!_updated && !force) {
+	if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
@@ -62,30 +63,31 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	_mc_rotors.enableYawControl(!_tilts.hasYawControl());
 
 	// Update matrix with tilts in vertical position when update is triggered by a manual
-	// configuration (parameter) change (=force update). This is to make sure the normalization
+	// configuration (parameter) change. This is to make sure the normalization
-	// scales are tilt-invariant. Note: force update only possible when disarm.
+	// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
-	const float tilt_control_applied = force ? -1.f : _last_tilt_control;
+	const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f :
+					   _last_tilt_control;
 	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied)
 			    << configuration.num_actuators[(int)ActuatorType::MOTORS];
 
-	_mc_rotors.getEffectivenessMatrix(configuration, true);
+	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
 
 	// Control Surfaces
 	configuration.selected_matrix = 1;
 	_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
-	_control_surfaces.getEffectivenessMatrix(configuration, true);
+	const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
 
 	// Tilts
 	configuration.selected_matrix = 0;
 	_first_tilt_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
 	_tilts.updateTorqueSign(_mc_rotors.geometry(), true /* disable pitch to avoid configuration errors */);
-	_tilts.getEffectivenessMatrix(configuration, true);
+	const bool tilts_added_successfully = _tilts.addActuators(configuration);
 
-	// If it was a forced update (thus coming from a config change), then make sure to update matrix in
+	// If it was an update coming from a config change, then make sure to update matrix in
 	// the next iteration again with the correct tilt (but without updating the normalization scale).
-	_updated = force;
+	_combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
 
-	return true;
+	return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
 }
 
 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
@@ -114,7 +116,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				_last_tilt_control = control_tilt;
 
 			} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
-				_updated = true;
+				_combined_tilt_updated = true;
 				_last_tilt_control = control_tilt;
 			}
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -55,7 +55,7 @@ public:
 	ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
 	int numMatrices() const override { return 2; }
 
@@ -81,7 +81,7 @@ public:
 
 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
 protected:
-	bool _updated{true};
+	bool _combined_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 	ActuatorEffectivenessTilts _tilts;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp

@@ -84,12 +84,8 @@ void ActuatorEffectivenessTilts::updateParams()
 	}
 }
 
-bool ActuatorEffectivenessTilts::getEffectivenessMatrix(Configuration &configuration, bool force)
+bool ActuatorEffectivenessTilts::addActuators(Configuration &configuration)
 {
-	if (!force) {
-		return false;
-	}
-
 	for (int i = 0; i < _count; i++) {
 		configuration.addActuator(ActuatorType::SERVOS, _torque[i], Vector3f{});
 	}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp

@@ -67,7 +67,7 @@ public:
 	ActuatorEffectivenessTilts(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessTilts() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+	bool addActuators(Configuration &configuration);
 
 	const char *name() const override { return "Tilts"; }
 

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -42,6 +42,8 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessFixedWing.hpp
 	ActuatorEffectivenessMCTilt.cpp
 	ActuatorEffectivenessMCTilt.hpp
+	ActuatorEffectivenessMultirotor.cpp
+	ActuatorEffectivenessMultirotor.hpp
 	ActuatorEffectivenessTilts.cpp
 	ActuatorEffectivenessTilts.hpp
 	ActuatorEffectivenessRotors.cpp

src/modules/control_allocator/ControlAllocator.cpp

@@ -124,7 +124,7 @@ ControlAllocator::parameters_updated()
 		_control_allocation[i]->updateParameters();
 	}
 
-	update_effectiveness_matrix_if_needed(true);
+	update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE);
 }
 
 void
@@ -208,7 +208,7 @@ ControlAllocator::update_effectiveness_source()
 		switch (source) {
 		case EffectivenessSource::NONE:
 		case EffectivenessSource::MULTIROTOR:
-			tmp = new ActuatorEffectivenessRotors(this);
+			tmp = new ActuatorEffectivenessMultirotor(this);
 			break;
 
 		case EffectivenessSource::STANDARD_VTOL:
@@ -362,7 +362,7 @@ ControlAllocator::Run()
 	if (do_update) {
 		_last_run = now;
 
-		update_effectiveness_matrix_if_needed();
+		update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
 
 		// Set control setpoint vector(s)
 		matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
@@ -414,15 +414,16 @@ ControlAllocator::Run()
 }
 
 void
-ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
+ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {
 	ActuatorEffectiveness::Configuration config{};
 
-	if (!force && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates
+	if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE
+	    && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates
 		return;
 	}
 
-	if (_actuator_effectiveness->getEffectivenessMatrix(config, force)) {
+	if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) {
 		_last_effectiveness_update = hrt_absolute_time();
 
 		memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes,
@@ -496,7 +497,7 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
 			// Assign control effectiveness matrix
 			int total_num_actuators = config.num_actuators_matrix[i];
 			_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],
-					config.linearization_point[i], total_num_actuators, force);
+					config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
 		}
 
 		trims.timestamp = hrt_absolute_time();

src/modules/control_allocator/ControlAllocator.hpp

@@ -42,7 +42,7 @@
 #pragma once
 
 #include <ActuatorEffectiveness.hpp>
-#include <ActuatorEffectivenessRotors.hpp>
+#include <ActuatorEffectivenessMultirotor.hpp>
 #include <ActuatorEffectivenessStandardVTOL.hpp>
 #include <ActuatorEffectivenessTiltrotorVTOL.hpp>
 #include <ActuatorEffectivenessTailsitterVTOL.hpp>
@@ -125,7 +125,7 @@ private:
 	void update_allocation_method(bool force);
 	bool update_effectiveness_source();
 
-	void update_effectiveness_matrix_if_needed(bool force = false);
+	void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);
 
 	void publish_control_allocator_status();