dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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)
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp
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));

9
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
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};
};

2
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
View File

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

2
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
View File

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

10
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -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;
}

1
src/modules/control_allocator/ControlAllocator.hpp
View File

@ -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,