diff --git a/Tools/module_config/generate_actuators_metadata.py b/Tools/module_config/generate_actuators_metadata.py index 196119259b..5d38ecf499 100755 --- a/Tools/module_config/generate_actuators_metadata.py +++ b/Tools/module_config/generate_actuators_metadata.py @@ -238,6 +238,7 @@ def get_actuator_output(yaml_config, output_functions, timer_config_file, verbos ( 'disarmed', 'Disarmed', 'DIS', False ), ( 'min', 'Minimum', 'MIN', 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 92b2593656..ecc29a4519 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). +''' + center_description = \ +'''Servo Center 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 ), + ( 'center', 'Center', 'CENT', center_description ), ( 'max', 'Maximum', 'MAX', maximum_description ), ( 'failsafe', 'Failsafe', 'FAIL', failsafe_description ), ] @@ -312,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/docs/assets/config/actuators/pwm_center_output.png b/docs/assets/config/actuators/pwm_center_output.png new file mode 100644 index 0000000000..ef83e96d88 Binary files /dev/null and b/docs/assets/config/actuators/pwm_center_output.png differ diff --git a/docs/assets/config/actuators/servo_pwm_center.png b/docs/assets/config/actuators/servo_pwm_center.png new file mode 100644 index 0000000000..c1d92b7676 Binary files /dev/null and b/docs/assets/config/actuators/servo_pwm_center.png differ diff --git a/docs/assets/config/actuators/servo_pwm_linear.png b/docs/assets/config/actuators/servo_pwm_linear.png new file mode 100644 index 0000000000..176beab77e Binary files /dev/null and b/docs/assets/config/actuators/servo_pwm_linear.png differ diff --git a/docs/en/config/actuators.md b/docs/en/config/actuators.md index 3eec8f2dd2..56af61c737 100644 --- a/docs/en/config/actuators.md +++ b/docs/en/config/actuators.md @@ -159,7 +159,7 @@ The fields are: - `Yaw Torque`: Effectiveness of actuator around yaw axis (normalised: -1 to 1). [Generally you should use the default actuator value](#actuator-roll-pitch-and-yaw-scaling). - `Trim`: An offset added to the actuator so that it is centered without input. - This might be determined by trial and error. + This might be determined by trial and error. Prefer using the improved `PWM_CENT` instead: [PWM control surfaces](./actuators.md#pwm-control-surfaces-that-move-both-directions-about-a-neutral-point) - (Advanced) `Slew Rate`: Limits the minimum time in which the motor/servo signal is allowed to pass through its full output range, in seconds. - The setting limits the rate of change of an actuator (if not specified then no rate limit is applied). It is intended for actuators that may be damaged or cause flight disturbance if they move too fast — such as the tilting actuators on a tiltrotor VTOL vehicle, or fast moving flaps, respectively. @@ -535,12 +535,40 @@ If you're using PWM servos, PWM50 is far more common. If a high rate servo is _really_ needed, DShot offers better value. ::: -#### Control surfaces that move both directions about a neutral point +##### PWM: Control surfaces that move both directions about a neutral point + +To facilitate setting the neutral point of the servos, a bilinear curve function can be defined using the following parameters `PWM_MAIM_CENTx` / `PWM_AUX_CENTx` for each servo. This allows for unequal deflections in the positive and negative direction: +![Asymetric Servo Deflections](../../assets/config/actuators/servo_pwm_center.png) + +To set this up: + +1. Set all surface `Trim` to `0.00` for all surfaces: + + ![PWM Trimming](../../assets/config/actuators/control_surface_trim.png) + +1. Set the `PWM_MAIN_CENTx` / `PWM_AUX_CENTx` value so that the surface will stay at the neutral (aligned with airfoil) position. +This is usually around `1500` for PWM servos (near the center of the servo range). + +![Control Surface Trimming](../../assets/config/actuators/pwm_center_output.png) + +2. Gradualy increase the `Maximum` for each servo until the desired deflection is reached. Check the deflection with a remote manual mode while [`COM_PREARM_MODE`](../advanced_config/parameter_reference.md#COM_PREARM_MODE) is set to `Always` or use the sliders. +3. Gradualy decrease the `Minimum` for each servo, until the desired deflection is reached. +4. Set `Disarmed` value to the desired value. It is usually desirable to have it the same as the `Center` value. + +::: info +If you want to retain the linear behaviour of the servo after setting the `Center`, make sure to adjust the `Minimum` or `Maximum`, such that both invervals (`min` to `cent` & `cent` to `max`) are equally lare. + +![Linear PWM Adjustment](../../assets/config/actuators/servo_pwm_linear.png) +::: + +#### Non-PWM: Control surfaces that move both directions about a neutral point Control surfaces that move either direction around a neutral point include: ailerons, elevons, V-tails, A-tails, and rudders. To set these up: +0. Set all `PWM_MAIN_CENTx` and `PWM_AUX_CENTx` to default (-1), or trimming will not be possible. + 1. Set the `Disarmed` value so that the surfaces will stay at neutral position when disarmed. This is usually around `1500` for PWM servos (near the centre of the servo range). @@ -559,14 +587,20 @@ To set these up: 3. Move the slider again to the middle and check if the Control Surfaces are aligned in the neutral position of the wing. - If it is not aligned, you can set the **Trim** value for the control surface. - ::: info - This is done in the `Trim` setting of the Geometry panel, usually by "trial and error". - ![Control Surface Trimming](../../assets/config/actuators/control_surface_trim.png) - ::: + ::: info + This is done in the `Trim` setting of the Geometry panel, usually by "trial and error". + ![Control Surface Trimming](../../assets/config/actuators/control_surface_trim.png) + ::: - After setting the trim for a control surface, move its slider away from the centre, release, and then back into disarmed (middle) position. Confirm that surface is in the neutral position. + + +::: tip +If any servo has a `PWM_MAIN_CENTx` or `PWM_AUX_CENTx` not set to default (-1), the system will automatically remove `Trim` from all surfaces. This is done to prevent mixing of old and new trimming tools. +::: + ::: info Another way to test without using the sliders would be to set the [`COM_PREARM_MODE`](../advanced_config/parameter_reference.md#COM_PREARM_MODE) parameter to `Always`: @@ -575,6 +609,8 @@ Another way to test without using the sliders would be to set the [`COM_PREARM_M ::: + + #### Control surfaces that move from neutral to full deflection Control surfaces that move only one direction from neutral include: airbrakes, spoilers, and flaps. @@ -585,6 +621,7 @@ For a flap, that is when the flap is fully retracted and flush with the wing. One approach for setting these up is: +0. Set all `PWM_MAIN_CENTx` and `PWM_AUX_CENTx` to default (-1), or trimming will not be possible. 1. Set values `Disarmed` to `1500`, `Min` to `1200`, `Max` to `1700` so that the values are around the centre of the servo range. 2. Move the corresponding slider up and check the control moves and that it is extending (moving away from the disarmed position). If not, click on the `Rev Range` checkbox to reverse the range. @@ -594,6 +631,7 @@ One approach for setting these up is: - If the value was increased towards `Max`, then set `Max` to match `Disarmed`. 4. The value that you did _not_ set to match `Disarmed` controls the maximum amount that the control surface can extend. Set the slider to the top of the control, then change the value (`Max` or `Min`) so that the control surface is fully extended when the slider is at top. +5. (Only PWM servos) Set the `Center` value to the middle between `Min` and `Max`. ::: info Special note for flaps In some vehicle builds, flaps may be configured such that both flaps are controlled from a single output. @@ -631,6 +669,9 @@ For each of the tilt servos: - Standard VTOL : Motors defined as multicopter motors will be turned off - Tiltrotors : Motors that have no associated tilt servo will turn off - Tailsitters do not turn off any motors in fixed-wing flight +- The following formula can be used to migrate from surface trim to PWM trim: + + `PWM_MAIN_CENTx = ((PWM_MAX - PWM_MIN) / 2) * CA_SV_CSx_TRIM + PWM_MIN + ((PWM_MAX - PWM_MIN) / 2)` ### Reversing Motors diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index 8c2049cceb..2c5e54e83d 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -8,6 +8,7 @@ actuator_output: disarmed: { min: 800, max: 2200, default: 1000 } min: { min: 800, max: 1400, default: 1000 } max: { min: 1600, max: 2200, default: 2000 } + center: { min: 800, max: 2200} 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 b038268d84..fe1b818f86 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, "CENT", i + instance_start); + _param_handles[i].center = 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, center: %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], _center_value[i]); } } @@ -173,6 +175,10 @@ void MixingOutput::updateParams() _min_value[i] = val; } + if (_param_handles[i].center != PARAM_INVALID && param_get(_param_handles[i].center, &val) == 0) { + _center_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,7 @@ void MixingOutput::updateParams() _max_value[i] = tmp; } + if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { _failsafe_value[i] = val; } @@ -372,6 +379,14 @@ void MixingOutput::setAllMinValues(uint16_t value) } } +void MixingOutput::setAllCenterValues(uint16_t value) +{ + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _param_handles[i].center = PARAM_INVALID; + _center_value[i] = value; + } +} + void MixingOutput::setAllMaxValues(uint16_t value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { @@ -528,10 +543,34 @@ 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 + && _param_handles[i].center != PARAM_INVALID + && _center_value[i] >= 800 + && _center_value[i] <= 2200) { + + /* bi-linear interpolation */ + if (value < 0.0f) { + output = math::interpolate(value, -1.f, 0.0f, + static_cast(_min_value[i]), static_cast(_center_value[i])); + + } else { + output = math::interpolate(value, 0.0f, 1.0f, + static_cast(_center_value[i]), static_cast(_max_value[i])); + } + + } + + // 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])); + } 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..4fdc0f2904 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 setAllCenterValues(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 ¢erValue(int index) { return _center_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 centerParamHandle(int index) const { return _param_handles[index].center; } 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 center{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 _center_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..7f3a6feede 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 CENTER_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.setAllCenterValues(CENTER_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.setAllCenterValues(CENTER_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.setAllCenterValues(CENTER_VALUE); // Set center 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.setAllCenterValues(10); // Set center 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.setAllCenterValues(10); // Set center 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/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 46fea5f33e..f6e216b855 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include +#include #include "ActuatorEffectivenessControlSurfaces.hpp" @@ -74,6 +75,40 @@ void ActuatorEffectivenessControlSurfaces::updateParams() return; } + // 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); + param_t param = param_find(param_name); + + if (param != PARAM_INVALID) + { + int32_t value; + + 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; + }; + + // Check if any PWM_MAIN or PWM_AUX center is configured + bool pwm_center_set = false; + + 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++) { param_get(_param_handles[i].type, (int32_t *)&_params[i].type); @@ -84,6 +119,20 @@ 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) > FLT_EPSILON) { + /* EVENT + * @description Display warning in GCS when TRIM settings were present and now CENTER are set. + */ + events::send(events::ID("control_surfaces_reset_trim"), events::Log::Warning, + "CA_SV_CS{1}_TRIM ({2}) is reset to 0 as PWM CENTER is used", i, _params[i].trim); + + _params[i].trim = 0.0f; + // Update the parameter storage + param_set(_param_handles[i].trim, &_params[i].trim); + } + param_get(_param_handles[i].scale_flap, &_params[i].scale_flap); param_get(_param_handles[i].scale_spoiler, &_params[i].scale_spoiler); diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 99d0d5ed85..dd36a2d070 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -312,7 +312,11 @@ parameters: CA_SV_CS${i}_TRIM: description: short: Control Surface ${i} trim - long: Can be used to add an offset to the servo control. + long: | + Can be used to add an offset to the servo control. + + NOTE: Do not use for PWM servos. Use the PWM CENTER parameters instead (e.g., PWM_MAIN_CENT, PWM_AUX_CENT) instead. + This parameter can only be set if all PWM Center parameters are set to default. type: float decimal: 2 min: -1.0 diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 1e1baa1037..1f17bf5fb2 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 + center: + type: dict + schema: + min: + # Minimum center value + type: integer + min: 0 + max: 65536 + max: + # Maximum center value + type: integer + min: 0 + max: 65536 + default: + # Default center 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: