From 3702140e24c2dcc66212dc4bde86f732ac4d3e7a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 4 May 2021 06:59:33 -0700 Subject: [PATCH] PWMOut:Arm once all channels are initalized. --- src/drivers/pwm_out/PWMOut.cpp | 18 ++++++++++++++---- src/drivers/pwm_out/PWMOut.hpp | 2 +- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index f01de16c86..a8b00e9c10 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -35,6 +35,7 @@ pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER; static px4::atomic _objects[PWM_OUT_MAX_INSTANCES] {}; +static px4::atomic _all_instances_ready {0}; static bool is_running() { @@ -114,6 +115,7 @@ int PWMOut::init() int PWMOut::set_mode(Mode mode) { unsigned old_mask = _pwm_mask; + bool old_pwm_initialized = _pwm_initialized; /* * Configure for PWM output. @@ -343,6 +345,10 @@ int PWMOut::set_mode(Mode mode) up_pwm_servo_deinit(old_mask); _pwm_on = false; } + + if (old_pwm_initialized != _pwm_initialized) { + _all_instances_ready.fetch_sub(1); + } } _mode = mode; @@ -537,7 +543,7 @@ void PWMOut::capture_callback(uint32_t chan_index, fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow); } -void PWMOut::update_pwm_out_state(bool on) +bool PWMOut::update_pwm_out_state(bool on) { if (on && !_pwm_initialized && _pwm_mask != 0) { @@ -551,7 +557,6 @@ void PWMOut::update_pwm_out_state(bool on) pwm_mask_new |= _objects[i].load()->get_pwm_mask(); pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels(); - } } @@ -562,10 +567,13 @@ void PWMOut::update_pwm_out_state(bool on) // Set rate is not affecting non-masked channels, so can be called // individually set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate()); + _pwm_initialized = true; + _all_instances_ready.fetch_add(1); } up_pwm_servo_arm(on, _pwm_mask); + return _all_instances_ready.load() == PWM_OUT_MAX_INSTANCES; } bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -619,8 +627,10 @@ void PWMOut::Run() bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.armed().in_esc_calibration_mode; if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - update_pwm_out_state(pwm_on); + + if (update_pwm_out_state(pwm_on)) { + _pwm_on = pwm_on; + } } // check for parameter updates diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index bb2437dec1..9b84d5115e 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -221,7 +221,7 @@ private: int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); - void update_pwm_out_state(bool on); + bool update_pwm_out_state(bool on); void update_params();