From 46dbb7cf6376144efb3664082ce80f90af34eb16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 22 Nov 2022 11:59:34 +0100 Subject: [PATCH] fix pwm_out, px4io: prevent disarm and rate param updates during boot Before, the logic to update disarm and rate values also triggered during bootup on the px4io, because the output functions are only set in updateSubscriptions(). Therefore change the check to prevent updating during the first cycle. --- src/drivers/pwm_out/PWMOut.cpp | 5 ++--- src/drivers/pwm_out/PWMOut.hpp | 2 +- src/drivers/px4io/px4io.cpp | 10 ++++------ 3 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index c3d82c1370..3a93e0e9d6 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -190,6 +190,7 @@ void PWMOut::Run() _mixing_output.updateSubscriptions(true); perf_end(_cycle_perf); + _first_update_cycle = false; } int PWMOut::task_spawn(int argc, char *argv[]) @@ -218,7 +219,7 @@ void PWMOut::update_params() updateParams(); // Automatically set the PWM rate and disarmed value when a channel is first set to a servo - if (!_first_param_update) { + if (!_first_update_cycle) { 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; @@ -257,8 +258,6 @@ void PWMOut::update_params() } } } - - _first_param_update = false; } int PWMOut::custom_command(int argc, char *argv[]) diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 4a1c21892f..55d498a8fc 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -91,7 +91,7 @@ private: bool _pwm_on{false}; uint32_t _pwm_mask{0}; bool _pwm_initialized{false}; - bool _first_param_update{true}; + bool _first_update_cycle{true}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 05d58e4b37..70d8ec6004 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -177,7 +177,7 @@ private: unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO - bool _first_param_update{true}; + bool _first_update_cycle{true}; uint32_t _group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {}; hrt_abstime _poll_last{0}; @@ -638,7 +638,9 @@ void PX4IO::Run() // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) _mixing_output.updateSubscriptions(true); + perf_end(_cycle_perf); + _first_update_cycle = false; } void PX4IO::updateTimerRateGroups() @@ -696,7 +698,7 @@ void PX4IO::update_params() if (!_mixing_output.armed().armed) { // Automatically set the PWM rate and disarmed value when a channel is first set to a servo - if (!_first_param_update) { + if (!_first_update_cycle) { 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; @@ -740,11 +742,7 @@ void PX4IO::update_params() updateTimerRateGroups(); updateFailsafe(); updateDisarmed(); - _first_param_update = false; - return; } - - _first_param_update = false; } void PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result)