mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-09 16:38:52 +08:00
move clamping and reorder values
This commit is contained in:
@@ -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:
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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]));
|
||||
}
|
||||
|
||||
+17
-5
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user