fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 22:20:35 +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/ControlAllocator.cpp
+6 -4
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;
}