control_allocator: major refactoring & additions

- allow effectiveness matrix to select control allocator method
  (desaturation algorithm)
- add actuator_servos publication
- add support for multiple matrices (for vtol)
- add updateSetpoint callback method to actuator effectiveness to allow it
  to manipulate the actuator setpoint after allocation
- handle motor stopping & reversal
- add control surfaces & tilt servos
- handle standard vtol + tiltrotor
- rename MC rotors params & class to be more generically usable
- fixes and enables ActuatorEffectivenessRotorsTest

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -71,14 +71,16 @@
 
 #include <matrix/matrix/math.hpp>
 
+#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp"
+
 class ControlAllocation
 {
 public:
 	ControlAllocation();
 	virtual ~ControlAllocation() = default;
 
-	static constexpr uint8_t NUM_ACTUATORS = 16;
-	static constexpr uint8_t NUM_AXES = 6;
+	static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS;
+	static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES;
 
 	typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
 
@@ -93,8 +95,6 @@ public:
 
 	/**
 	 * Allocate control setpoint to actuators
-	 *
-	 * @param control_setpoint  Desired control setpoint vector (input)
 	 */
 	virtual void allocate() = 0;
 
@@ -190,6 +190,8 @@ public:
 	 */
 	void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
 
+	void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); }
+
 	/**
 	 * Normalize the actuator setpoint between minimum and maximum values.
 	 *
@@ -207,6 +209,8 @@ public:
 	int numConfiguredActuators() const { return _num_actuators; }
 
 protected:
+	friend class ControlAllocator; // for _actuator_sp
+
 	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  //< Effectiveness matrix
 	matrix::Vector<float, NUM_AXES> _control_allocation_scale;  	//< Scaling applied during allocation
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; 	//< Neutral actuator values

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -122,7 +122,4 @@ ControlAllocationPseudoInverse::allocate()
 
 	// Allocate
 	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
-
-	// Clip
-	clipActuatorSetpoint(_actuator_sp);
 }

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp

@@ -53,9 +53,9 @@ public:
 	ControlAllocationPseudoInverse() = default;
 	virtual ~ControlAllocationPseudoInverse() = default;
 
-	virtual void allocate() override;
-	virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
-					    const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim, int num_actuators) override;
+	void allocate() override;
+	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
+				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim, int num_actuators) override;
 
 protected:
 	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp

@@ -59,6 +59,7 @@ TEST(ControlAllocationTest, AllZeroCase)
 	method.setEffectivenessMatrix(effectiveness, actuator_trim, 16);
 	method.setControlSetpoint(control_sp);
 	method.allocate();
+	method.clipActuatorSetpoint();
 	actuator_sp = method.getActuatorSetpoint();
 	control_allocated_expected = method.getAllocatedControl();
 

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -60,9 +60,6 @@ ControlAllocationSequentialDesaturation::allocate()
 		mixAirmodeDisabled();
 		break;
 	}
-
-	// Clip
-	clipActuatorSetpoint(_actuator_sp);
 }
 
 void ControlAllocationSequentialDesaturation::desaturateActuators(