fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 13:54:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Allocation: separate accessing shared state from slew limiting

Browse Source 
introducing more variables than strictly necessary to clearly specify
what is input and what is output.

no functional change
This commit is contained in:
Balduin 2026-02-18 09:45:49 +01:00
parent 9cf211f48c
commit 7170e638c5
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/control_allocation/control_allocation/ControlAllocation.cpp
View File

@ -112,15 +112,23 @@ void ControlAllocation::applySlewRateLimit(float dt)
{
for (int i = 0; i < _num_actuators; i++) {
if (_actuator_slew_rate_limit(i) > FLT_EPSILON) {
float input = _actuator_sp(i);
float previous = _prev_actuator_sp(i);
float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i);
float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i);
float delta_sp = input - previous;
float output = input;
if (delta_sp > delta_sp_max) {
_actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max;
output = previous + delta_sp_max;
} else if (delta_sp < -delta_sp_max) {
_actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max;
output = previous - delta_sp_max;
}
_actuator_sp(i) = output;
}
}
}