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

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

Browse Source 
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>
This commit is contained in:
Nick
2025-11-26 18:12:25 +01:00
committed by GitHub
co-authored by Silvan
parent 6caf4f0942
commit 575fa0850b
13 changed files with 187 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/pwm_out/module.yaml
+1
View File
@@ -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:
src/lib/mixer_module/mixer_module.cpp
+43 -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, "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
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 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
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 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);
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
+49
View File
@@ -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);
src/modules/control_allocator/module.yaml
+5 -1
View File
@@ -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