fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-25 13:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

control_allocator: limit status publication rate to 200Hz

Browse Source 
Reduces CPU load by ~3.5% on F4 @2khz.
And compute getAllocatedControl as needed (~1.5% CPU reduction)
This commit is contained in:
Beat Küng
2021-11-11 11:13:21 +01:00
committed by Daniel Agar
parent 93a54ee63d
commit b29d9db7f1
6 changed files with 10 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp
+1 -10
View File
@@ -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
+2 -7
View File
@@ -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
-2
View File
@@ -125,6 +125,4 @@ ControlAllocationPseudoInverse::allocate()
// Clip
clipActuatorSetpoint(_actuator_sp);
ControlAllocation::updateControlAllocated();
}
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
-2
View File
@@ -63,8 +63,6 @@ ControlAllocationSequentialDesaturation::allocate()
// Clip
clipActuatorSetpoint(_actuator_sp);
ControlAllocation::updateControlAllocated();
}
void ControlAllocationSequentialDesaturation::desaturateActuators(