MC mixer: replace multirotor_motor_limits by control_allocator_status

CA: fix saturation computation
Since the CA matrix is normalized, the same scale applied to be used when using the effectiveness matrix

MCRateControl: use control_allocator_status to get saturation info

src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp

@@ -68,9 +68,7 @@ ControlAllocation::setActuatorSetpoint(
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
 
-	// Compute achieved control
-	_control_allocated = _effectiveness * _actuator_sp;
-
+	updateControlAllocated();
 }
 
 void
@@ -89,6 +87,13 @@ ControlAllocation::clipActuatorSetpoint(matrix::Vector<float, ControlAllocation:
 	}
 }
 
+void
+ControlAllocation::updateControlAllocated()
+{
+	// Compute achieved control
+	_control_allocated = (_effectiveness * _actuator_sp).emult(_control_allocation_scale);
+}
+
 matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
 ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator)
 const

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -75,7 +75,7 @@
 class ControlAllocation
 {
 public:
-	ControlAllocation() = default;
+	ControlAllocation() { _control_allocation_scale.setAll(1.f); }
 	virtual ~ControlAllocation() = default;
 
 	static constexpr uint8_t NUM_ACTUATORS = 16;
@@ -190,6 +190,11 @@ public:
 	 */
 	void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
 
+	/**
+	 * Compute the amount of allocated control thrust and torque
+	 */
+	void updateControlAllocated();
+
 	/**
 	 * Normalize the actuator setpoint between minimum and maximum values.
 	 *
@@ -208,6 +213,7 @@ public:
 
 protected:
 	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
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_min; 	//< Minimum actuator values
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_max; 	//< Maximum actuator values

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -66,26 +66,28 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
 	// Same scale on roll and pitch
 	const float roll_norm_sq = _mix.col(0).norm_squared();
 	const float pitch_norm_sq = _mix.col(1).norm_squared();
-	const float roll_pitch_scale = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f));
+	_control_allocation_scale(0) = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f));
+	_control_allocation_scale(1) = _control_allocation_scale(0);
 
-	if (roll_pitch_scale > FLT_EPSILON) {
-		_mix.col(0) /= roll_pitch_scale;
-		_mix.col(1) /= roll_pitch_scale;
+	if (_control_allocation_scale(0) > FLT_EPSILON) {
+		_mix.col(0) /= _control_allocation_scale(0);
+		_mix.col(1) /= _control_allocation_scale(1);
 	}
 
 	// Scale yaw separately
-	const float yaw_scale = _mix.col(2).max();
+	_control_allocation_scale(2) = _mix.col(2).max();
 
-	if (yaw_scale > FLT_EPSILON) {
-		_mix.col(2) /= yaw_scale;
+	if (_control_allocation_scale(2) > FLT_EPSILON) {
+		_mix.col(2) /= _control_allocation_scale(2);
 	}
 
 	// Same scale on X and Y
-	const float xy_scale = fmaxf(_mix.col(3).max(), _mix.col(4).max());
+	_control_allocation_scale(3) = fmaxf(_mix.col(3).max(), _mix.col(4).max());
+	_control_allocation_scale(4) = _control_allocation_scale(3);
 
-	if (xy_scale > FLT_EPSILON) {
-		_mix.col(3) /= xy_scale;
-		_mix.col(4) /= xy_scale;
+	if (_control_allocation_scale(3) > FLT_EPSILON) {
+		_mix.col(3) /= _control_allocation_scale(3);
+		_mix.col(4) /= _control_allocation_scale(4);
 	}
 
 	// Scale Z thrust separately
@@ -97,7 +99,8 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
 	}
 
 	if ((-z_sum > FLT_EPSILON) && (_num_actuators > 0)) {
-		_mix.col(5) /= -z_sum / _num_actuators;
+		_control_allocation_scale(5) = -z_sum / _num_actuators;
+		_mix.col(5) /= _control_allocation_scale(5);
 	}
 
 	// Set all the small elements to 0 to avoid issues
@@ -123,6 +126,5 @@ ControlAllocationPseudoInverse::allocate()
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
 
-	// Compute achieved control
-	_control_allocated = _effectiveness * _actuator_sp;
+	ControlAllocation::updateControlAllocated();
 }

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -66,8 +66,7 @@ ControlAllocationSequentialDesaturation::allocate()
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
 
-	// Compute achieved control
-	_control_allocated = _effectiveness * _actuator_sp;
+	ControlAllocation::updateControlAllocated();
 }
 
 void ControlAllocationSequentialDesaturation::desaturateActuators(

src/modules/control_allocator/ControlAllocator.hpp

@@ -112,7 +112,6 @@ private:
 	void publish_control_allocator_status();
 
 	void publish_legacy_actuator_controls();
-	void publish_legacy_multirotor_motor_limits();
 
 	enum class AllocationMethod {
 		NONE = -1,