From b29d9db7f1ef6ea1ca6ca5ca3816ed7d99965eee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 11 Nov 2021 11:13:21 +0100 Subject: [PATCH] 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) --- .../ControlAllocation/ControlAllocation.cpp | 11 +---------- .../ControlAllocation/ControlAllocation.hpp | 9 ++------- .../ControlAllocationPseudoInverse.cpp | 2 -- .../ControlAllocationSequentialDesaturation.cpp | 2 -- src/modules/control_allocator/ControlAllocator.cpp | 10 ++++++---- src/modules/control_allocator/ControlAllocator.hpp | 1 + 6 files changed, 10 insertions(+), 25 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index b219e16ee7..751dd5e5bf 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/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 ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector &actuator) const { matrix::Vector 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)); diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index e075e0006b..67441b9ffb 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -132,7 +132,8 @@ public: * * @return Control vector */ - const matrix::Vector &getAllocatedControl() const { return _control_allocated; } + matrix::Vector getAllocatedControl() const + { return (_effectiveness * _actuator_sp).emult(_control_allocation_scale); } /** * Get the control effectiveness matrix @@ -189,11 +190,6 @@ public: */ void clipActuatorSetpoint(matrix::Vector &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 _actuator_max; //< Maximum actuator values matrix::Vector _actuator_sp; //< Actuator setpoint matrix::Vector _control_sp; //< Control setpoint - matrix::Vector _control_allocated; //< Allocated control matrix::Vector _control_trim; //< Control at trim actuator values int _num_actuators{0}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index ef1245941e..ea78ed391f 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -125,6 +125,4 @@ ControlAllocationPseudoInverse::allocate() // Clip clipActuatorSetpoint(_actuator_sp); - - ControlAllocation::updateControlAllocated(); } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 8cc0679e5e..5132579a7f 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -63,8 +63,6 @@ ControlAllocationSequentialDesaturation::allocate() // Clip clipActuatorSetpoint(_actuator_sp); - - ControlAllocation::updateControlAllocated(); } void ControlAllocationSequentialDesaturation::desaturateActuators( diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 4420d4f496..354302f022 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/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 actuator_sp = _control_allocation->getActuatorSetpoint(); + const matrix::Vector &actuator_sp = _control_allocation->getActuatorSetpoint(); matrix::Vector 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; } diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 47e47ddc98..75f3719fe7 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/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) _param_ca_airframe,