pwm_out, dshot: add dynamic mixing support

Open-UAS · Oct 18, 2021 · c1e5e66 · c1e5e66
1 parent 3ff6014
commit c1e5e66
Show file tree

Hide file tree

Showing 6 changed files with 302 additions and 65 deletions.
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/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

diff --git a/src/drivers/dshot/DShot.cpp b/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);

diff --git a/src/drivers/pwm_out/CMakeLists.txt b/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