From f1acf76726b24e695cf924b2acc06af1d2bea4f9 Mon Sep 17 00:00:00 2001 From: ttechnick Date: Tue, 11 Nov 2025 15:51:53 +0100 Subject: [PATCH] PWM: Add servo trim option --- .../generate_actuators_metadata.py | 1 + Tools/module_config/generate_params.py | 4 ++ docs/en/config/actuators.md | 22 +++++++++ src/drivers/pwm_out/module.yaml | 1 + src/lib/mixer_module/mixer_module.cpp | 49 +++++++++++++++++-- src/lib/mixer_module/mixer_module.hpp | 5 ++ src/lib/mixer_module/mixer_module_tests.cpp | 6 +++ validation/module_schema.yaml | 22 +++++++++ 8 files changed, 106 insertions(+), 4 deletions(-) diff --git a/Tools/module_config/generate_actuators_metadata.py b/Tools/module_config/generate_actuators_metadata.py index 196119259b..d18a2db34c 100755 --- a/Tools/module_config/generate_actuators_metadata.py +++ b/Tools/module_config/generate_actuators_metadata.py @@ -237,6 +237,7 @@ def get_actuator_output(yaml_config, output_functions, timer_config_file, verbos ( 'function', 'Function', 'FUNC', False ), ( 'disarmed', 'Disarmed', 'DIS', False ), ( 'min', 'Minimum', 'MIN', False ), + ( 'trim', 'Trim', 'TRIM', False ), ( 'max', 'Maximum', 'MAX', False ), ( 'failsafe', 'Failsafe', 'FAIL', True ), ] diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 92b2593656..e05781b0da 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -284,6 +284,9 @@ Note that non-motor outputs might already be active in prearm state if COM_PREAR ''' minimum_description = \ '''Minimum output value (when not disarmed). +''' + trim_description = \ +'''Trim output value (when not disarmed). ''' maximum_description = \ '''Maxmimum output value (when not disarmed). @@ -296,6 +299,7 @@ When set to -1 (default), the value depends on the function (see {:}). standard_params_array = [ ( 'disarmed', 'Disarmed', 'DIS', disarmed_description ), ( 'min', 'Minimum', 'MIN', minimum_description ), + ( 'trim', 'Trim', 'TRIM', trim_description ), ( 'max', 'Maximum', 'MAX', maximum_description ), ( 'failsafe', 'Failsafe', 'FAIL', failsafe_description ), ] diff --git a/docs/en/config/actuators.md b/docs/en/config/actuators.md index 3eec8f2dd2..a48a446c7c 100644 --- a/docs/en/config/actuators.md +++ b/docs/en/config/actuators.md @@ -575,6 +575,28 @@ Another way to test without using the sliders would be to set the [`COM_PREARM_M ::: +##### Split Servo Deflections + +::: info +This method is experimental, but much faster. + +::: +To facilitate setting the neutral point of the servos, a bilinear curve function can be defined using the following parameters `PWM_MAIM_TRIMx` / `PWM_AUX_TRIMx` for each servo. This allows for unequal deflections in the positive and negative direction: +![Split Servo Deflections](../../assets/config/actuators/servo_pwm_trim.png) + +To set this up: + +1. Set all surface `Trim` to `0.00` for all surfaces: + + ![Control Surface Trimming](../../assets/config/actuators/control_surface_trim.png) + +1. Set `Minimum`, `Maximum`, `Trim` and `Disarm` value so that the surface will stay at neutral position approximately. +This is usually around `1500` for PWM servos (near the centre of the servo range). + +2. Gradualy increate the `Maximum` for each servo, while checking the deflection with the sliders or manual input with [`COM_PREARM_MODE`](../advanced_config/parameter_reference.md#COM_PREARM_MODE) parameter to `Always` until the desired deflection is reached. +3. Gradualy decrease the `Minimum` for each servo, until the desired deflection is reached. +4. Set `PWM_MAIN_TRIMx` / `PWM_AUX_TRIMx` to the neutral point of each servo. + #### Control surfaces that move from neutral to full deflection Control surfaces that move only one direction from neutral include: airbrakes, spoilers, and flaps. diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index 8c2049cceb..0fccc39f88 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -7,6 +7,7 @@ actuator_output: standard_params: disarmed: { min: 800, max: 2200, default: 1000 } min: { min: 800, max: 1400, default: 1000 } + trim: { min: 800, max: 2200, default: 1500 } max: { min: 1600, max: 2200, default: 2000 } failsafe: { min: 800, max: 2200 } extra_function_groups: [ pwm_fmu ] diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index b038268d84..b42904d03d 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -120,6 +120,8 @@ void MixingOutput::initParamHandles(const uint8_t instance_start) _param_handles[i].disarmed = param_find(param_name); snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MIN", i + instance_start); _param_handles[i].min = param_find(param_name); + snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "TRIM", i + instance_start); + _param_handles[i].trim = param_find(param_name); snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MAX", i + instance_start); _param_handles[i].max = param_find(param_name); snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FAIL", i + instance_start); @@ -142,9 +144,9 @@ void MixingOutput::printStatus() const PX4_INFO_RAW("Channel Configuration:\n"); for (unsigned i = 0; i < _max_num_outputs; i++) { - PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i, + PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d, trim: %d\n", i, (int)_function_assignment[i], _current_output_value[i], - actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]); + actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i], _trim_value[i]); } } @@ -173,6 +175,10 @@ void MixingOutput::updateParams() _min_value[i] = val; } + if (_param_handles[i].trim != PARAM_INVALID && param_get(_param_handles[i].trim, &val) == 0) { + _trim_value[i] = val; + } + if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) { _max_value[i] = val; } @@ -183,6 +189,15 @@ void MixingOutput::updateParams() _max_value[i] = tmp; } + // Trim needs to be clamped to min/max + if (_trim_value[i] < _min_value[i]) { + _trim_value[i] = _min_value[i]; + } + + if (_trim_value[i] > _max_value[i]) { + _trim_value[i] = _max_value[i]; + } + if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { _failsafe_value[i] = val; } @@ -372,6 +387,14 @@ void MixingOutput::setAllMinValues(uint16_t value) } } +void MixingOutput::setAllTrimValues(uint16_t value) +{ + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _param_handles[i].trim = PARAM_INVALID; + _trim_value[i] = value; + } +} + void MixingOutput::setAllMaxValues(uint16_t value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { @@ -528,10 +551,28 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const value = -1.f * value; } - const float output = math::interpolate(value, -1.f, 1.f, - static_cast(_min_value[i]), static_cast(_max_value[i])); + float output = _disarmed_value[i]; + + if (_function_assignment[i] >= OutputFunction::Servo1 + && _function_assignment[i] <= OutputFunction::ServoMax) { + + /* bi-linear interpolation */ + if (value < 0.0f) { + output = math::interpolate(value, -1.f, 0.0f, + static_cast(_min_value[i]), static_cast(_trim_value[i])); + + } else { + output = math::interpolate(value, 0.0f, 1.0f, + static_cast(_trim_value[i]), static_cast(_max_value[i])); + } + + } else { + output = math::interpolate(value, -1.f, 1.f, + static_cast(_min_value[i]), static_cast(_max_value[i])); + } return math::constrain(lroundf(output), 0L, static_cast(UINT16_MAX)); + } void diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 7ed8ed8a73..75616ec606 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -167,16 +167,19 @@ public: void setAllFailsafeValues(uint16_t value); void setAllDisarmedValues(uint16_t value); void setAllMinValues(uint16_t value); + void setAllTrimValues(uint16_t value); void setAllMaxValues(uint16_t value); /** Disarmed values: disarmedValue < minValue needs to hold */ uint16_t &disarmedValue(int index) { return _disarmed_value[index]; } uint16_t &minValue(int index) { return _min_value[index]; } + uint16_t &trimValue(int index) { return _trim_value[index]; } uint16_t &maxValue(int index) { return _max_value[index]; } param_t functionParamHandle(int index) const { return _param_handles[index].function; } param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; } param_t minParamHandle(int index) const { return _param_handles[index].min; } + param_t trimParamHandle(int index) const { return _param_handles[index].trim; } param_t maxParamHandle(int index) const { return _param_handles[index].max; } /** @@ -228,6 +231,7 @@ private: param_t function{PARAM_INVALID}; param_t disarmed{PARAM_INVALID}; param_t min{PARAM_INVALID}; + param_t trim{PARAM_INVALID}; param_t max{PARAM_INVALID}; param_t failsafe{PARAM_INVALID}; }; @@ -240,6 +244,7 @@ private: uint16_t _failsafe_value[MAX_ACTUATORS] {}; uint16_t _disarmed_value[MAX_ACTUATORS] {}; uint16_t _min_value[MAX_ACTUATORS] {}; + uint16_t _trim_value[MAX_ACTUATORS] {}; uint16_t _max_value[MAX_ACTUATORS] {}; uint16_t _current_output_value[MAX_ACTUATORS] {}; ///< current output values (reordered) uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction diff --git a/src/lib/mixer_module/mixer_module_tests.cpp b/src/lib/mixer_module/mixer_module_tests.cpp index 7db9a8c513..d6d5f07930 100644 --- a/src/lib/mixer_module/mixer_module_tests.cpp +++ b/src/lib/mixer_module/mixer_module_tests.cpp @@ -54,6 +54,7 @@ static constexpr int MAX_NUM_OUTPUTS = 8; static constexpr int DISARMED_VALUE = 900; static constexpr int FAILSAFE_VALUE = 800; static constexpr int MIN_VALUE = 1000; +static constexpr int TRIM_VALUE = 1500; static constexpr int MAX_VALUE = 2000; class MixerModuleTest : public ::testing::Test @@ -188,6 +189,7 @@ TEST_F(MixerModuleTest, basic) mixing_output.setAllDisarmedValues(DISARMED_VALUE); mixing_output.setAllFailsafeValues(FAILSAFE_VALUE); mixing_output.setAllMinValues(MIN_VALUE); + mixing_output.setAllTrimValues(TRIM_VALUE); mixing_output.setAllMaxValues(MAX_VALUE); EXPECT_EQ(test_module.num_updates, 0); @@ -281,6 +283,7 @@ TEST_F(MixerModuleTest, arming) mixing_output.setAllDisarmedValues(DISARMED_VALUE); mixing_output.setAllFailsafeValues(FAILSAFE_VALUE); mixing_output.setAllMinValues(MIN_VALUE); + mixing_output.setAllTrimValues(TRIM_VALUE); mixing_output.setAllMaxValues(MAX_VALUE); test_module.sendMotors({1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f}); @@ -488,6 +491,7 @@ TEST_F(MixerModuleTest, OutputLimitCalcSingle) mixing_output.setAllMinValues(MIN_VALUE); // default range [1000,2000] mixing_output.setAllMaxValues(MAX_VALUE); + mixing_output.setAllTrimValues(1500); // Set trim to middle value EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.f), 1000); // In range EXPECT_EQ(mixing_output.output_limit_calc_single(0, -.5f), 1250); EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.f), 1500); @@ -503,6 +507,7 @@ TEST_F(MixerModuleTest, OutputLimitCalcSingle) mixing_output.setAllMinValues(0); // lower range [0,20] mixing_output.setAllMaxValues(20); + mixing_output.setAllTrimValues(10); // Set trim to middle value EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.f), 0); // In range EXPECT_EQ(mixing_output.output_limit_calc_single(0, -.5f), 5); EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.f), 10); @@ -518,6 +523,7 @@ TEST_F(MixerModuleTest, OutputLimitCalcSingle) mixing_output.setAllMinValues(20); // inverted range [20,0] mixing_output.setAllMaxValues(0); + mixing_output.setAllTrimValues(10); // Set trim to middle value EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.f), 20); // In range EXPECT_EQ(mixing_output.output_limit_calc_single(0, -.5f), 15); EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.f), 10); diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 1e1baa1037..9dbd39c492 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -352,6 +352,28 @@ actuator_output: # ui only shows the param if this condition is true type: string regex: *condition_regex + trim: + type: dict + schema: + min: + # Minimum trim value + type: integer + min: 0 + max: 65536 + max: + # Maximum trim value + type: integer + min: 0 + max: 65536 + default: + # Default trim value + type: integer + min: 0 + max: 65536 + show_if: + # ui only shows the param if this condition is true + type: string + regex: *condition_regex failsafe: type: dict schema: