control_allocator: limit status publication rate to 200Hz

Reduces CPU load by ~3.5% on F4 @2khz.
And compute getAllocatedControl as needed (~1.5% CPU reduction)

src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp

@@ -67,8 +67,6 @@ ControlAllocation::setActuatorSetpoint(
 
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
-
-	updateControlAllocated();
 }
 
 void
@@ -87,20 +85,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
 {
 	matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> actuator_normalized;
 
-	for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) {
+	for (int i = 0; i < _num_actuators; i++) {
 		if (_actuator_min(i) < _actuator_max(i)) {
 			actuator_normalized(i) = (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
 

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -132,7 +132,8 @@ public:
 	 *
 	 * @return Control vector
 	 */
-	const matrix::Vector<float, NUM_AXES> &getAllocatedControl() const { return _control_allocated; }
+	matrix::Vector<float, NUM_AXES> getAllocatedControl() const
+	{ return (_effectiveness * _actuator_sp).emult(_control_allocation_scale); }
 
 	/**
 	 * Get the control effectiveness matrix
@@ -189,11 +190,6 @@ 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.
 	 *
@@ -218,7 +214,6 @@ protected:
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_max; 	//< Maximum actuator values
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_sp;  	//< Actuator setpoint
 	matrix::Vector<float, NUM_AXES> _control_sp;   		//< Control setpoint
-	matrix::Vector<float, NUM_AXES> _control_allocated;  	//< Allocated control
 	matrix::Vector<float, NUM_AXES> _control_trim;  	//< Control at trim actuator values
 	int _num_actuators{0};
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -125,6 +125,4 @@ ControlAllocationPseudoInverse::allocate()
 
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
-
-	ControlAllocation::updateControlAllocated();
 }

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -63,8 +63,6 @@ ControlAllocationSequentialDesaturation::allocate()
 
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
-
-	ControlAllocation::updateControlAllocated();
 }
 
 void ControlAllocationSequentialDesaturation::desaturateActuators(

src/modules/control_allocator/ControlAllocator.cpp

@@ -336,9 +336,11 @@ ControlAllocator::Run()
 
 		// Publish actuator setpoint and allocator status
 		publish_actuator_controls();
-		publish_control_allocator_status();
-
 
+		if (now - _last_status_pub >= 5_ms) {
+			publish_control_allocator_status();
+			_last_status_pub = now;
+		}
 	}
 
 	perf_end(_loop_perf);
@@ -424,11 +426,11 @@ ControlAllocator::publish_actuator_controls()
 	actuator_motors.timestamp = hrt_absolute_time();
 	actuator_motors.timestamp_sample = _timestamp_sample;
 
-	matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation->getActuatorSetpoint();
 	matrix::Vector<float, NUM_ACTUATORS> actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(
 				actuator_sp);
 
-	for (size_t i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
+	for (int i = 0; i < _control_allocation->numConfiguredActuators(); i++) {
 		actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
 	}
 

src/modules/control_allocator/ControlAllocator.hpp

@@ -155,6 +155,7 @@ private:
 
 	hrt_abstime _last_run{0};
 	hrt_abstime _timestamp_sample{0};
+	hrt_abstime _last_status_pub{0};
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,