From fa76f04560dae46bf2c593199d792b4fff8a5efc Mon Sep 17 00:00:00 2001 From: henrykotze Date: Mon, 13 Nov 2023 18:27:06 +0200 Subject: [PATCH] Trim params for Tilts Conform to Tilt naming scheme Update description for trim tilts update tilt parameter name to conform --- .../ActuatorEffectivenessTilts.cpp | 9 ++++++++- .../ActuatorEffectivenessTilts.hpp | 2 ++ src/modules/control_allocator/module.yaml | 15 +++++++++++++++ 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp index 3cce6b24fb8e..1347d003e667 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp @@ -52,6 +52,8 @@ ActuatorEffectivenessTilts::ActuatorEffectivenessTilts(ModuleParams *parent) _param_handles[i].max_angle = param_find(buffer); snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_TD", i); _param_handles[i].tilt_direction = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_TRIM", i); + _param_handles[i].trim = param_find(buffer); } _count_handle = param_find("CA_SV_TL_COUNT"); @@ -76,6 +78,7 @@ void ActuatorEffectivenessTilts::updateParams() param_get(_param_handles[i].tilt_direction, (int32_t *)&_params[i].tilt_direction); param_get(_param_handles[i].min_angle, &_params[i].min_angle); param_get(_param_handles[i].max_angle, &_params[i].max_angle); + param_get(_param_handles[i].trim, &_params[i].trim); _params[i].min_angle = math::radians(_params[i].min_angle); _params[i].max_angle = math::radians(_params[i].max_angle); @@ -87,7 +90,11 @@ void ActuatorEffectivenessTilts::updateParams() bool ActuatorEffectivenessTilts::addActuators(Configuration &configuration) { for (int i = 0; i < _count; i++) { - configuration.addActuator(ActuatorType::SERVOS, _torque[i], Vector3f{}); + int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _torque[i], Vector3f{}); + + if (actuator_idx >= 0) { + configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim; + } } return true; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index d885a091552b..a2320d4ff2b3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -62,6 +62,7 @@ class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffective float min_angle; float max_angle; TiltDirection tilt_direction; + float trim; }; ActuatorEffectivenessTilts(ModuleParams *parent); @@ -89,6 +90,7 @@ class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffective param_t min_angle; param_t max_angle; param_t tilt_direction; + param_t trim; }; ParamHandles _param_handles[MAX_COUNT]; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 2c506cc4e270..7202b6a51c12 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -411,6 +411,19 @@ parameters: instance_start: 0 default: 0 + + CA_SV_TL${i}_TRIM: + description: + short: Tilt Servo ${i} trim + long: Can be used to add an offset to the servo control. + type: float + decimal: 2 + min: -1.0 + max: 1.0 + num_instances: *max_num_tilts + instance_start: 0 + default: 0.0 + # helicopter CA_SP0_COUNT: description: @@ -1015,6 +1028,8 @@ mixer: label: 'Tilt Direction' - name: 'CA_SV_TL${i}_CT' label: 'Use for Control' + - name: 'CA_SV_TL${i}_TRIM' + label: 'Trim' 9: # Custom actuators: