diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 9c8ecd8c9a..c1aa29e7f2 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -531,12 +531,43 @@ void PWMOut::capture_callback(uint32_t chan_index, void PWMOut::update_pwm_out_state(bool on) { if (on && !_pwm_initialized && _pwm_mask != 0) { - up_pwm_servo_init(_pwm_mask); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + // Collect all PWM masks from all instances + uint32_t pwm_mask_new = 0; + // Collect the PWM alt rate channels across all instances + uint32_t pwm_alt_rate_channels_new = 0; + + // Collect the minimum default rate + unsigned default_rate_min = 0; + // Collect the maximum alternative rate (400 Hz or DSHOT outputs) + unsigned alt_rate_max = 0; + + for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { + if (_objects[i].load()) { + pwm_mask_new |= _objects[i].load()->get_pwm_mask(); + pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels(); + + if (_objects[i].load()->get_alt_rate() > alt_rate_max) { + alt_rate_max = _objects[i].load()->get_alt_rate(); + } + + if (_objects[i].load()->get_default_rate() < default_rate_min) { + default_rate_min = _objects[i].load()->get_default_rate(); + } + } + } + + // Initialize the PWM output state for all instances + // this is re-done once per instance, but harmless + up_pwm_servo_init(pwm_mask_new); + + // Set rate is not affecting non-masked channels, so can be called + // individually + set_pwm_rate(pwm_alt_rate_channels_new, default_rate_min, alt_rate_max); _pwm_initialized = true; } - up_pwm_servo_arm(on); // TODO REVIEW for multi + up_pwm_servo_arm(on); } bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 20ca599ee5..86707b28bf 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -159,6 +159,10 @@ public: int set_mode(Mode mode); Mode get_mode() { return _mode; } + uint32_t get_pwm_mask() { return _pwm_mask; } + uint32_t get_alt_rate_channels() { return _pwm_alt_rate_channels; } + unsigned get_alt_rate() { return _pwm_alt_rate; } + unsigned get_default_rate() { return _pwm_default_rate; } void request_mode(Mode new_mode); static int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);