From 264d53efbe28c600c9fd6ef9e57f0d73f6664b07 Mon Sep 17 00:00:00 2001 From: ttechnick Date: Thu, 20 Nov 2025 13:20:37 +0100 Subject: [PATCH] move clamping and reorder values --- .../generate_actuators_metadata.py | 2 +- Tools/module_config/generate_params.py | 6 ++++- src/drivers/pwm_out/module.yaml | 2 +- src/lib/mixer_module/mixer_module.cpp | 11 ++++++---- .../ActuatorEffectivenessControlSurfaces.cpp | 22 ++++++++++++++----- 5 files changed, 31 insertions(+), 12 deletions(-) diff --git a/Tools/module_config/generate_actuators_metadata.py b/Tools/module_config/generate_actuators_metadata.py index 91714d9327..5d38ecf499 100755 --- a/Tools/module_config/generate_actuators_metadata.py +++ b/Tools/module_config/generate_actuators_metadata.py @@ -237,8 +237,8 @@ def get_actuator_output(yaml_config, output_functions, timer_config_file, verbos ( 'function', 'Function', 'FUNC', False ), ( 'disarmed', 'Disarmed', 'DIS', False ), ( 'min', 'Minimum', 'MIN', False ), - ( 'center', 'Center', 'CENT', False ), ( 'max', 'Maximum', 'MAX', False ), + ( 'center', 'Center\n(for Servos)', 'CENT', False ), ( 'failsafe', 'Failsafe', 'FAIL', True ), ] for key, label, param_suffix, advanced in standard_params_array: diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index d4d59b64d9..ecc29a4519 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -286,7 +286,7 @@ Note that non-motor outputs might already be active in prearm state if COM_PREAR '''Minimum output value (when not disarmed). ''' center_description = \ -'''Center output value (when not disarmed). +'''Servo Center output value (when not disarmed). ''' maximum_description = \ '''Maxmimum output value (when not disarmed). @@ -316,6 +316,10 @@ When set to -1 (default), the value depends on the function (see {:}). standard_params[key]['default'] = -1 standard_params[key]['min'] = -1 + if key == 'center': + standard_params[key]['default'] = -1 + standard_params[key]['min'] = -1 + param = { 'description': { 'short': channel_label+' ${i} '+label+' Value', diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index 3f63358800..ad0a93b48e 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -7,8 +7,8 @@ actuator_output: standard_params: disarmed: { min: 800, max: 2200, default: 1000 } min: { min: 800, max: 1400, default: 1000 } - center: { min: 800, max: 2200, default: 1500 } max: { min: 1600, max: 2200, default: 2000 } + center: { min: 800, max: 2200, default: -1 } failsafe: { min: 800, max: 2200 } extra_function_groups: [ pwm_fmu ] pwm_timer_param: diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 176948f474..fe1b818f86 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -189,8 +189,6 @@ void MixingOutput::updateParams() _max_value[i] = tmp; } - // Center needs to be clamped to min/max - _center_value[i] = math::constrain(_center_value[i], _min_value[i], _max_value[i]); if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { _failsafe_value[i] = val; @@ -549,7 +547,9 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const if (_function_assignment[i] >= OutputFunction::Servo1 && _function_assignment[i] <= OutputFunction::ServoMax - && _param_handles[i].center != PARAM_INVALID) { + && _param_handles[i].center != PARAM_INVALID + && _center_value[i] >= 800 + && _center_value[i] <= 2200) { /* bi-linear interpolation */ if (value < 0.0f) { @@ -561,7 +561,10 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const static_cast(_center_value[i]), static_cast(_max_value[i])); } - } else { + } + + // Everything except servos, or if center is not set + else { output = math::interpolate(value, -1.f, 1.f, static_cast(_min_value[i]), static_cast(_max_value[i])); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 5a35dea886..46f93f4e97 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -74,7 +74,7 @@ void ActuatorEffectivenessControlSurfaces::updateParams() return; } - // Helper to check if a PWM center parameter is set to non-default value (1500 µs) + // Helper to check if a PWM center parameter is enabled, and clamp it to valid range auto check_pwm_center = [](const char *prefix, int channel) -> bool { char param_name[20]; snprintf(param_name, sizeof(param_name), "%s_CENT%d", prefix, channel); @@ -83,7 +83,17 @@ void ActuatorEffectivenessControlSurfaces::updateParams() if (param != PARAM_INVALID) { int32_t value; - return (param_get(param, &value) == PX4_OK && value != 1500); + + if (param_get(param, &value) == PX4_OK && value != -1) { + // Clamp PWM center to valid range [800, 2200] + if (value < 800 || value > 2200) { + int32_t clamped = (value < 800) ? 800 : 2200; + PX4_WARN("%s_CENT%d (%d) out of range, clamping to %d", prefix, channel, (int)value, (int)clamped); + param_set(param, &clamped); + } + + return true; + } } return false; @@ -92,8 +102,10 @@ void ActuatorEffectivenessControlSurfaces::updateParams() // Check if any PWM_MAIN or PWM_AUX center is configured bool pwm_center_set = false; - for (int i = 1; i <= 8 && !pwm_center_set; i++) { - pwm_center_set = check_pwm_center("PWM_MAIN", i) || check_pwm_center("PWM_AUX", i); + for (int i = 1; i <= 8; i++) { + if (check_pwm_center("PWM_MAIN", i) || check_pwm_center("PWM_AUX", i)) { + pwm_center_set = true; + } } for (int i = 0; i < _count; i++) { @@ -108,7 +120,7 @@ void ActuatorEffectivenessControlSurfaces::updateParams() param_get(_param_handles[i].trim, &_params[i].trim); // If PWM center is set and CA_SV_CS trim is non-zero, warn and reset to 0 - if (pwm_center_set && fabsf(_params[i].trim) > 0.001f) { + if (pwm_center_set && fabsf(_params[i].trim) > FLT_EPSILON) { PX4_WARN("CA_SV_CS%d_TRIM (%.3f) should be 0 when PWM CENTER is used. Resetting to 0.", i, (double)_params[i].trim); _params[i].trim = 0.0f;