fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 05:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

PWM: Add servo trim option

Browse Source
This commit is contained in:
ttechnick
2025-11-11 15:53:14 +01:00
parent 98d3a2141a
commit f1acf76726
8 changed files with 106 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/module_config/generate_actuators_metadata.py
+1
View File
@@ -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 ),
]
Tools/module_config/generate_params.py
+4
View File
@@ -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 ),
]
docs/en/config/actuators.md
+22
View File
@@ -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.
src/drivers/pwm_out/module.yaml
+1
View File
@@ -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 ]
src/lib/mixer_module/mixer_module.cpp
+45 -4
View File
@@ -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<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) {
/* bi-linear interpolation */
if (value < 0.0f) {
output = math::interpolate(value, -1.f, 0.0f,
static_cast<float>(_min_value[i]), static_cast<float>(_trim_value[i]));
} else {
output = math::interpolate(value, 0.0f, 1.0f,
static_cast<float>(_trim_value[i]), static_cast<float>(_max_value[i]));
}
} 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
src/lib/mixer_module/mixer_module.hpp
+5
View File
@@ -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
src/lib/mixer_module/mixer_module_tests.cpp
+6
View File
@@ -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);
validation/module_schema.yaml
+22
View File
@@ -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: