From bf09089a26b9b7672efc12a467fe6795cb81d471 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Tue, 13 Apr 2021 14:51:41 +0200
Subject: [PATCH] PWM out: Collect settings for all outputs There are settings
that have to be the same for a split output across multiple instances, like
for example the PWM mask and rate configurations. This change collects them
for all outputs of the same underlying driver structure and applies the
complete set.
---
src/drivers/pwm_out/PWMOut.cpp | 37 +++++++++++++++++++++++++++++++---
src/drivers/pwm_out/PWMOut.hpp | 4 ++++
2 files changed, 38 insertions(+), 3 deletions(-)
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);