From e7588d2da0906b7b3838cd6c8fb18b3d7bec9d7e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 29 Jul 2022 14:01:35 +0200 Subject: [PATCH] px4io+pwm_out: set the PWM rate and disarmed value when a channel is first set to a servo This should simplify the first setup a bit. --- src/drivers/pwm_out/PWMOut.cpp | 49 +++++++++++++++++++++ src/drivers/pwm_out/PWMOut.hpp | 1 + src/drivers/px4io/px4io.cpp | 63 ++++++++++++++++++++++++++- src/lib/mixer_module/mixer_module.hpp | 3 ++ 4 files changed, 114 insertions(+), 2 deletions(-) diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 682942a985..b5a43f1a7b 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -494,8 +494,57 @@ void PWMOut::Run() void PWMOut::update_params() { + uint32_t previously_set_functions = 0; + + for (size_t i = 0; i < _num_outputs; i++) { + previously_set_functions |= (uint32_t)_mixing_output.isFunctionSet(i) << i; + } + updateParams(); + // Automatically set the PWM rate and disarmed value when a channel is first set to a servo + if (_mixing_output.useDynamicMixing() && !_first_param_update) { + for (size_t i = 0; i < _num_outputs; i++) { + if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) { + int32_t output_function; + + if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0 + && output_function >= (int)OutputFunction::Servo1 + && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo + int32_t val = 1500; + PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i); + param_set(_mixing_output.disarmedParamHandle(i), &val); + + // If the whole timer group was not set previously, then set the pwm rate to 50 Hz + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + + uint32_t channels = io_timer_get_group(timer); + + if ((channels & (1u << i)) == 0) { + continue; + } + + if ((channels & previously_set_functions) == 0) { // None of the channels was set + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); + + int32_t tim_config = 0; + param_t handle = param_find(param_name); + + if (param_get(handle, &tim_config) == 0 && tim_config == 400) { + tim_config = 50; + PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config); + param_set(handle, &tim_config); + } + } + } + } + } + } + } + + _first_param_update = false; + if (_mixing_output.useDynamicMixing()) { return; } diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 87c4f91ebe..de2ba67282 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -147,6 +147,7 @@ private: bool _pwm_on{false}; uint32_t _pwm_mask{0}; bool _pwm_initialized{false}; + bool _first_param_update{true}; unsigned _num_disarmed_set{0}; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0319a2dada..016d1ce128 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -179,6 +179,8 @@ private: unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO int _class_instance{-1}; + bool _first_param_update{true}; + uint32_t _group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {}; hrt_abstime _poll_last{0}; @@ -477,6 +479,12 @@ int PX4IO::init() return ret; } + /* initialize _group_channels */ + for (uint8_t group = PX4IO_P_SETUP_PWM_RATE_GROUP0; group <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++group) { + unsigned group_idx = group - PX4IO_P_SETUP_PWM_RATE_GROUP0; + _group_channels[group_idx] = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + group_idx); + } + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) { _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); @@ -605,8 +613,6 @@ void PX4IO::Run() _param_update_force = false; - ModuleParams::updateParams(); - update_params(); /* Check if the flight termination circuit breaker has been updated */ @@ -699,14 +705,67 @@ void PX4IO::updateTimerRateGroups() void PX4IO::update_params() { + uint32_t previously_set_functions = 0; + + for (size_t i = 0; i < _max_actuators; i++) { + previously_set_functions |= (uint32_t)_mixing_output.isFunctionSet(i) << i; + } + + updateParams(); + if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) { + + // Automatically set the PWM rate and disarmed value when a channel is first set to a servo + if (!_first_param_update) { + for (size_t i = 0; i < _max_actuators; i++) { + if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) { + int32_t output_function; + + if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0 + && output_function >= (int)OutputFunction::Servo1 + && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo + int32_t val = 1500; + PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i); + param_set(_mixing_output.disarmedParamHandle(i), &val); + + // If the whole timer group was not set previously, then set the pwm rate to 50 Hz + for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) { + + uint32_t channels = _group_channels[timer]; + + if ((channels & (1u << i)) == 0) { + continue; + } + + if ((channels & previously_set_functions) == 0) { // None of the channels was set + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); + + int32_t tim_config = 0; + param_t handle = param_find(param_name); + + if (param_get(handle, &tim_config) == 0 && tim_config == 400) { + tim_config = 50; + PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config); + param_set(handle, &tim_config); + } + } + } + } + } + } + } + // sync params to IO updateTimerRateGroups(); updateFailsafe(); updateDisarmed(); + _first_param_update = false; return; } + _first_param_update = false; + // skip update when armed or PWM disabled if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) { return; diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 1c16ec6b2e..63c0d410df 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -193,6 +193,9 @@ public: uint16_t &minValue(int index) { return _min_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; } + /** * Returns the actual failsafe value taking into account the assigned function */