pwm_out, dshot: add dynamic mixing support

ROMFS/px4fmu_common/init.d/rc.interface

@@ -96,10 +96,16 @@ then
 
 	if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ]
 	then
-		if ! $OUTPUT_CMD start
+		if param compare SYS_CTRL_ALLOC 1
 		then
-			echo "$OUTPUT_CMD start failed"
-			tune_control play error
+			pwm_out start
+			dshot start
+		else
+			if ! $OUTPUT_CMD start
+			then
+				echo "$OUTPUT_CMD start failed"
+				tune_control play error
+			fi
 		fi
 	fi
 fi
@@ -214,10 +220,10 @@ fi
 
 if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ]
 then
-	if [ $PWM_OUT != none ]
+	if [ $PWM_OUT != none -a $PWM_MAIN_RATE != none ]
 	then
 		# Set PWM output frequency.
-		if [ $PWM_MAIN_RATE != none ]
+		if ! param compare SYS_CTRL_ALLOC 1
 		then
 			pwm rate -c ${PWM_OUT} -r ${PWM_MAIN_RATE}
 		fi

src/drivers/dshot/DShot.cpp

@@ -33,6 +33,8 @@
 
 #include "DShot.h"
 
+#include <px4_arch/io_timer.h>
+
 char DShot::_telemetry_device[] {};
 px4::atomic_bool DShot::_request_telemetry_init{false};
 
@@ -43,7 +45,7 @@ DShot::DShot() :
 	_mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE);
 	_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
 	_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
-
+	_mixing_output.setAllFailsafeValues(UINT16_MAX);
 }
 
 DShot::~DShot()
@@ -114,39 +116,112 @@ int DShot::task_spawn(int argc, char *argv[])
 void DShot::enable_dshot_outputs(const bool enabled)
 {
 	if (enabled && !_outputs_initialized) {
-		DShotConfig config = (DShotConfig)_param_dshot_config.get();
+		if (_mixing_output.useDynamicMixing()) {
 
-		unsigned int dshot_frequency = DSHOT600;
+			unsigned int dshot_frequency = 0;
+			uint32_t dshot_frequency_param = 0;
 
-		switch (config) {
-		case DShotConfig::DShot150:
-			dshot_frequency = DSHOT150;
-			break;
+			for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+				uint32_t channels = io_timer_get_group(timer);
 
-		case DShotConfig::DShot300:
-			dshot_frequency = DSHOT300;
-			break;
+				if (channels == 0) {
+					continue;
+				}
 
-		case DShotConfig::DShot600:
-			dshot_frequency = DSHOT600;
-			break;
+				char param_name[17];
+				snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
 
-		case DShotConfig::DShot1200:
-			dshot_frequency = DSHOT1200;
-			break;
+				int32_t tim_config = 0;
+				param_t handle = param_find(param_name);
+				param_get(handle, &tim_config);
+				unsigned int dshot_frequency_request = 0;
 
-		default:
-			break;
-		}
+				if (tim_config == -5) {
+					dshot_frequency_request = DSHOT150;
 
-		int ret = up_dshot_init(_output_mask, dshot_frequency);
+				} else if (tim_config == -4) {
+					dshot_frequency_request = DSHOT300;
 
-		if (ret < 0) {
-			PX4_ERR("up_dshot_init failed (%i)", ret);
-			return;
+				} else if (tim_config == -3) {
+					dshot_frequency_request = DSHOT600;
+
+				} else if (tim_config == -2) {
+					dshot_frequency_request = DSHOT1200;
+
+				} else {
+					_output_mask &= ~channels; // don't use for dshot
+				}
+
+				if (dshot_frequency_request != 0) {
+					if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
+						PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name);
+						param_set_no_notification(handle, &dshot_frequency_param);
+
+					} else {
+						dshot_frequency = dshot_frequency_request;
+						dshot_frequency_param = tim_config;
+					}
+				}
+			}
+
+			int ret = up_dshot_init(_output_mask, dshot_frequency);
+
+			if (ret < 0) {
+				PX4_ERR("up_dshot_init failed (%i)", ret);
+				return;
+			}
+
+			_output_mask = ret;
+
+			// disable unused functions
+			for (unsigned i = 0; i < _num_outputs; ++i) {
+				if (((1 << i) & _output_mask) == 0) {
+					_mixing_output.disableFunction(i);
+				}
+			}
+
+			if (_output_mask == 0) {
+				// exit the module if no outputs used
+				request_stop();
+				return;
+			}
+
+		} else {
+			DShotConfig config = (DShotConfig)_param_dshot_config.get();
+
+			unsigned int dshot_frequency = DSHOT600;
+
+			switch (config) {
+			case DShotConfig::DShot150:
+				dshot_frequency = DSHOT150;
+				break;
+
+			case DShotConfig::DShot300:
+				dshot_frequency = DSHOT300;
+				break;
+
+			case DShotConfig::DShot600:
+				dshot_frequency = DSHOT600;
+				break;
+
+			case DShotConfig::DShot1200:
+				dshot_frequency = DSHOT1200;
+				break;
+
+			default:
+				break;
+			}
+
+			int ret = up_dshot_init(_output_mask, dshot_frequency);
+
+			if (ret < 0) {
+				PX4_ERR("up_dshot_init failed (%i)", ret);
+				return;
+			}
+
+			_output_mask = ret;
 		}
 
-		_output_mask = ret;
 		_outputs_initialized = true;
 	}
 
@@ -414,7 +489,7 @@ void DShot::Run()
 	_mixing_output.update();
 
 	// update output status if armed or if mixer is loaded
-	bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers();
+	bool outputs_on = _mixing_output.armed().armed || _mixing_output.initialized();
 
 	if (_outputs_on != outputs_on) {
 		enable_dshot_outputs(outputs_on);

src/drivers/pwm_out/CMakeLists.txt

@@ -37,6 +37,8 @@ px4_add_module(
 	SRCS
 		PWMOut.cpp
 		PWMOut.hpp
+	MODULE_CONFIG
+		module.yaml
 	DEPENDS
 		arch_io_pins
 		mixer

src/drivers/pwm_out/PWMOut.cpp

@@ -56,8 +56,10 @@ PWMOut::PWMOut(int instance, uint8_t output_base) :
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
 {
-	_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
-	_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
+	if (!_mixing_output.useDynamicMixing()) {
+		_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
+		_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
+	}
 }
 
 PWMOut::~PWMOut()
@@ -92,7 +94,13 @@ int PWMOut::init()
 
 	_mixing_output.setDriverInstance(_class_instance);
 
-	_num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE);
+	if (_mixing_output.useDynamicMixing()) {
+		_num_outputs = FMU_MAX_ACTUATORS;
+
+	} else {
+		_num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE);
+	}
+
 	_pwm_mask = ((1u << _num_outputs) - 1) << _output_base;
 	_mixing_output.setMaxNumOutputs(_num_outputs);
 
@@ -132,6 +140,10 @@ int PWMOut::init()
  */
 int PWMOut::set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate)
 {
+	if (_mixing_output.useDynamicMixing()) {
+		return -EINVAL;
+	}
+
 	PX4_DEBUG("pwm_out%u set_pwm_rate %x %u %u", _instance, rate_map, default_rate, alt_rate);
 
 	for (unsigned pass = 0; pass < 2; pass++) {
@@ -264,6 +276,11 @@ int PWMOut::task_spawn(int argc, char *argv[])
 					return PX4_ERROR;
 				}
 
+				// only start one instance with dynamic mixing
+				if (dev->_mixing_output.useDynamicMixing()) {
+					break;
+				}
+
 			} else {
 				PX4_ERR("alloc failed");
 			}
@@ -283,44 +300,110 @@ bool PWMOut::update_pwm_out_state(bool on)
 {
 	if (on && !_pwm_initialized && _pwm_mask != 0) {
 
-		// 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;
+		if (_mixing_output.useDynamicMixing()) {
+
+			for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+				_timer_rates[timer] = -1;
+
+				uint32_t channels = io_timer_get_group(timer);
+
+				if (channels == 0) {
+					continue;
+				}
+
+				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);
+				param_get(handle, &tim_config);
+
+				if (tim_config > 0) {
+					_timer_rates[timer] = tim_config;
 
-		for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
-			if (_objects[i].load()) {
+				} else if (tim_config == -1) { // OneShot
+					_timer_rates[timer] = 0;
 
-				pwm_mask_new |= _objects[i].load()->get_pwm_mask();
-				pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels();
+				} else {
+					_pwm_mask &= ~channels; // don't use for pwm
+				}
+			}
+
+			int ret = up_pwm_servo_init(_pwm_mask);
+
+			if (ret < 0) {
+				PX4_ERR("up_pwm_servo_init failed (%i)", ret);
+				return false;
+			}
+
+			_pwm_mask = ret;
+
+			// set the timer rates
+			for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+				uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
+
+				if (channels == 0) {
+					continue;
+				}
+
+				ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
+
+				if (ret != 0) {
+					PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
+					_timer_rates[timer] = -1;
+					_pwm_mask &= ~channels;
+				}
+			}
+
+			_pwm_initialized = true;
+
+			// disable unused functions
+			for (unsigned i = 0; i < _num_outputs; ++i) {
+				if (((1 << i) & _pwm_mask) == 0) {
+					_mixing_output.disableFunction(i);
+				}
 			}
-		}
 
-		// Initialize the PWM output state for all instances
-		// this is re-done once per instance, but harmless
-		int ret = up_pwm_servo_init(pwm_mask_new);
+		} else {
+			// 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;
 
-		if (ret >= 0) {
 			for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
 				if (_objects[i].load()) {
-					_objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret);
+
+					pwm_mask_new |= _objects[i].load()->get_pwm_mask();
+					pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels();
 				}
 			}
 
-			// 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());
+			// Initialize the PWM output state for all instances
+			// this is re-done once per instance, but harmless
+			int ret = up_pwm_servo_init(pwm_mask_new);
 
-			_pwm_initialized = true;
-			_all_instances_ready.fetch_add(1);
+			if (ret >= 0) {
+				for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
+					if (_objects[i].load()) {
+						_objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret);
+					}
+				}
 
-		} else {
-			PX4_ERR("up_pwm_servo_init failed (%i)", ret);
+				// 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);
+
+			} else {
+				PX4_ERR("up_pwm_servo_init failed (%i)", ret);
+			}
 		}
 	}
 
 	up_pwm_servo_arm(on, _pwm_mask);
-	return _all_instances_ready.load() == PWM_OUT_MAX_INSTANCES;
+	return _all_instances_ready.load() == PWM_OUT_MAX_INSTANCES || _mixing_output.useDynamicMixing();
 }
 
 bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
@@ -332,7 +415,12 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 	/* output to the servos */
 	if (_pwm_initialized) {
-		for (size_t i = 0; i < math::min(_num_outputs, num_outputs); i++) {
+		for (size_t i = 0; i < num_outputs; i++) {
+			if (!_mixing_output.isFunctionSet(i)) {
+				// do not run any signal on disabled channels
+				outputs[i] = 0;
+			}
+
 			if (_pwm_mask & (1 << (i + _output_base))) {
 				up_pwm_servo_set(_output_base + i, outputs[i]);
 			}
@@ -362,13 +450,16 @@ void PWMOut::Run()
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 
-	// push backup schedule
-	ScheduleDelayed(_backup_schedule_interval_us);
+	if (!_mixing_output.useDynamicMixing()) {
+		// push backup schedule
+		ScheduleDelayed(_backup_schedule_interval_us);
+	}
 
 	_mixing_output.update();
 
 	/* update PWM status if armed or if disarmed PWM values are set */
-	bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.armed().in_esc_calibration_mode;
+	bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing()
+		      || _mixing_output.armed().in_esc_calibration_mode;
 
 	if (_pwm_on != pwm_on) {
 
@@ -384,10 +475,12 @@ void PWMOut::Run()
 		_parameter_update_sub.copy(&pupdate);
 
 		// update parameters from storage
-		// update_params(); // do not update PWM params for now (was interfering with VTOL PWM settings)
+		if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings)
+			update_params();
+		}
 	}
 
-	if (_pwm_initialized && _current_update_rate == 0) {
+	if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) {
 		update_current_rate();
 	}
 
@@ -401,6 +494,10 @@ void PWMOut::update_params()
 {
 	updateParams();
 
+	if (_mixing_output.useDynamicMixing()) {
+		return;
+	}
+
 	int32_t pwm_min_default = PWM_DEFAULT_MIN;
 	int32_t pwm_max_default = PWM_DEFAULT_MAX;
 	int32_t pwm_disarmed_default = 0;
@@ -636,7 +733,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
 			/* discard if too many values are sent */
-			if (pwm->channel_count > FMU_MAX_ACTUATORS) {
+			if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
 				ret = -EINVAL;
 				break;
 			}
@@ -681,7 +778,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
 			/* discard if too many values are sent */
-			if (pwm->channel_count > FMU_MAX_ACTUATORS) {
+			if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
 				ret = -EINVAL;
 				break;
 			}
@@ -736,7 +833,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
 			/* discard if too many values are sent */
-			if (pwm->channel_count > FMU_MAX_ACTUATORS) {
+			if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
 				ret = -EINVAL;
 				break;
 			}
@@ -781,7 +878,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
 			/* discard if too many values are sent */
-			if (pwm->channel_count > FMU_MAX_ACTUATORS) {
+			if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
 				ret = -EINVAL;
 				break;
 			}
@@ -1223,6 +1320,27 @@ int PWMOut::print_status()
 	perf_print_counter(_interval_perf);
 	_mixing_output.printStatus();
 
+	if (_mixing_output.useDynamicMixing() && _pwm_initialized) {
+		for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+			if (_timer_rates[timer] >= 0) {
+				PX4_INFO_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]);
+				uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
+
+				if (channels > 0) {
+					PX4_INFO_RAW(" channels: ");
+
+					for (uint32_t channel = 0; channel < _num_outputs; ++channel) {
+						if ((1 << channel) & channels) {
+							PX4_INFO_RAW("%" PRIu32 " ", channel);
+						}
+					}
+				}
+
+				PX4_INFO_RAW("\n");
+			}
+		}
+	}
+
 	return 0;
 }
 

src/drivers/pwm_out/PWMOut.hpp

@@ -47,6 +47,7 @@
 #include <lib/mixer_module/mixer_module.hpp>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
+#include <px4_arch/io_timer.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/log.h>
@@ -137,6 +138,8 @@ private:
 	unsigned	_pwm_alt_rate{50};
 	uint32_t	_pwm_alt_rate_channels{0};
 
+	int _timer_rates[MAX_IO_TIMERS] {};
+
 	int		_current_update_rate{0};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

src/drivers/pwm_out/module.yaml

@@ -0,0 +1,33 @@
+module_name: PWM Output
+actuator_output:
+  output_groups:
+    - generator: pwm
+      param_prefix: PWM_FMU
+      channel_labels: ['PWM Main', 'PWM Capture']
+      standard_params: 
+        disarmed: { min: 800, max: 2200, default: 900 }
+        min: { min: 800, max: 1400, default: 1000 }
+        max: { min: 1600, max: 2200, default: 2000 }
+        failsafe: { min: 800, max: 2200 }
+      extra_function_groups: [ pwm_fmu ]
+      pwm_timer_param:
+        description:
+            short: Output Protocol Configuration for ${label}
+            long: |
+                Select which Output Protocol to use for outputs ${label}.
+
+                Custom PWM rates can be used by directly setting any value >0.
+        type: enum
+        default: 400
+        values:
+            -5: DShot150
+            -4: DShot300
+            -3: DShot600
+            -2: DShot1200
+            -1: OneShot
+            50: PWM50
+            100: PWM100
+            200: PWM200
+            400: PWM400
+        reboot_required: true
+