move clamping and reorder values

This commit is contained in:
ttechnick
2025-11-20 13:44:35 +01:00
parent b4eae779f5
commit 264d53efbe
5 changed files with 31 additions and 12 deletions
@@ -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:
+5 -1
View File
@@ -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',
+1 -1
View File
@@ -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:
+7 -4
View File
@@ -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<float>(_center_value[i]), static_cast<float>(_max_value[i]));
}
} else {
}
// Everything except servos, or if center is not set
else {
output = math::interpolate(value, -1.f, 1.f,
static_cast<float>(_min_value[i]), static_cast<float>(_max_value[i]));
}
@@ -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;