fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

control_allocator: implement trim + slew rate limits configuration

Browse Source
This commit is contained in:
Beat Küng
2021-12-24 20:06:13 -05:00
committed by Daniel Agar
parent 301100ce0e
commit 4c80adfaf1
17 changed files with 224 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
+9 -1
View File
@@ -50,6 +50,8 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul
_param_handles[i].torque[1] = param_find(buffer);
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_Y", i);
_param_handles[i].torque[2] = param_find(buffer);
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
_param_handles[i].trim = param_find(buffer);
}
_count_handle = param_find("CA_SV_CS_COUNT");
@@ -78,6 +80,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
param_get(_param_handles[i].torque[n], &torque(n));
}
param_get(_param_handles[i].trim, &_params[i].trim);
// TODO: enforce limits?
switch (_params[i].type) {
case Type::Elevator:
@@ -126,7 +130,11 @@ bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration
}
for (int i = 0; i < _count; i++) {
configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
if (actuator_idx >= 0) {
configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim;
}
}
return true;