PWM: Add servo center setting & asymetric deflection (#25897)

Add PWM_*_CENTERx for each servo. Use a bilinear transform to map actuator_servos to PWM signals. This solution only works for PWM based servos. Other types of servos are not affected. * PWM: Add servo trim option * PWM: Improve documentation of PWM trim feature * PWM: cleaner clamping and docs typo * update documentation & safety * add migration formula * rename param from trim to center * docs with center instead of trim * move clamping and reorder values * improve documentation * adress failing range check * improve documentation * CA: add event for setting CENTER with TRIM Signed-off-by: Silvan <silvan@auterion.com> --------- Signed-off-by: Silvan <silvan@auterion.com> Co-authored-by: Silvan <silvan@auterion.com>
2025-12-06 08:31:45 +08:00 · 2025-11-26 18:12:25 +01:00
parent 6caf4f0942
commit 575fa0850b
13 changed files with 187 additions and 11 deletions
--- 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:
--- 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',
--- a/docs/assets/config/actuators/pwm_center_output.png
+++ b/docs/assets/config/actuators/pwm_center_output.png
--- a/docs/assets/config/actuators/servo_pwm_center.png
+++ b/docs/assets/config/actuators/servo_pwm_center.png
--- a/docs/assets/config/actuators/servo_pwm_linear.png
+++ b/docs/assets/config/actuators/servo_pwm_linear.png
--- 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)
 - <a id="slew_rate"></a>(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

--- 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:
--- 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<float>(_min_value[i]), static_cast<float>(_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<float>(_min_value[i]), static_cast<float>(_center_value[i]));
+
+		} else {
+			output = math::interpolate(value, 0.0f, 1.0f,
+						   static_cast<float>(_center_value[i]), static_cast<float>(_max_value[i]));
+		}
+
+	}
+
+	// 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]));
+	}

 	return math::constrain(lroundf(output), 0L, static_cast<long>(UINT16_MAX));
+
 }

 void
--- 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 &centerValue(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
--- 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);
--- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
+++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
@@ -32,6 +32,7 @@
 ****************************************************************************/

 #include <px4_platform_common/log.h>
+#include <px4_platform_common/events.h>

 #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<uint8_t, float>(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);

--- 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
--- 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: