[drivers] Allow swapping RX/TX pins of DShot Telemetry

niklaut · niklaut · commit 8c6d7235e44a · 2025-05-27T11:07:29.000+02:00
diff --git a/boards/auterion/fmu-v6s/init/rc.board_defaults b/boards/auterion/fmu-v6s/init/rc.board_defaults
@@ -26,3 +26,6 @@ nshterm /dev/ttyS3 &
 
 # Start the time_persistor to cyclically store the RTC in FRAM
 time_persistor start
+
+# Start the ESC telemetry
+dshot telemetry -d /dev/ttyS5 -x
diff --git a/boards/hkust/nxt-dual/init/rc.board_extras b/boards/hkust/nxt-dual/init/rc.board_extras
@@ -10,4 +10,4 @@
 # fi
 
 # DShot telemetry is always on UART7
-# dshot telemetry /dev/ttyS5
+# dshot telemetry -d /dev/ttyS5
diff --git a/boards/hkust/nxt-v1/init/rc.board_extras b/boards/hkust/nxt-v1/init/rc.board_extras
@@ -10,4 +10,4 @@
 # fi
 
 # DShot telemetry is always on UART7
-# dshot telemetry /dev/ttyS5
+# dshot telemetry -d /dev/ttyS5
diff --git a/boards/holybro/kakutef7/init/rc.board_extras b/boards/holybro/kakutef7/init/rc.board_extras
@@ -9,4 +9,4 @@ then
 fi
 
 # DShot telemetry is always on UART7
-dshot telemetry /dev/ttyS5
+dshot telemetry -d /dev/ttyS5
diff --git a/boards/holybro/kakuteh7/init/rc.board_extras b/boards/holybro/kakuteh7/init/rc.board_extras
@@ -9,4 +9,4 @@ then
 fi
 
 # DShot telemetry is always on UART7
-dshot telemetry /dev/ttyS5
+dshot telemetry -d /dev/ttyS5
diff --git a/boards/holybro/kakuteh7dualimu/init/rc.board_extras b/boards/holybro/kakuteh7dualimu/init/rc.board_extras
@@ -9,4 +9,4 @@ then
 fi
 
 # DShot telemetry is always on UART7
-dshot telemetry /dev/ttyS5
+dshot telemetry -d /dev/ttyS5
diff --git a/boards/holybro/kakuteh7mini/init/rc.board_extras b/boards/holybro/kakuteh7mini/init/rc.board_extras
@@ -9,4 +9,4 @@ then
 fi
 
 # DShot telemetry is always on UART7
-dshot telemetry /dev/ttyS5
+dshot telemetry -d /dev/ttyS5
diff --git a/boards/holybro/kakuteh7v2/init/rc.board_extras b/boards/holybro/kakuteh7v2/init/rc.board_extras
@@ -9,4 +9,4 @@ then
 fi
 
 # DShot telemetry is always on UART7
-dshot telemetry /dev/ttyS5
+dshot telemetry -d /dev/ttyS5
diff --git a/boards/matek/h743-mini/init/rc.board_extras b/boards/matek/h743-mini/init/rc.board_extras
@@ -11,4 +11,4 @@
 atxxxx start -s
 
 # DShot telemetry is always on UART7
-# dshot telemetry /dev/ttyS5
+# dshot telemetry -d /dev/ttyS5
diff --git a/boards/matek/h743-slim/init/rc.board_extras b/boards/matek/h743-slim/init/rc.board_extras
@@ -12,4 +12,4 @@ atxxxx start -s
 
 
 # DShot telemetry is always on UART7
-# dshot telemetry /dev/ttyS5
+# dshot telemetry -d /dev/ttyS5
diff --git a/boards/matek/h743/init/rc.board_extras b/boards/matek/h743/init/rc.board_extras
@@ -12,4 +12,4 @@ atxxxx start -s
 
 
 # DShot telemetry is always on UART7
-# dshot telemetry /dev/ttyS5
+# dshot telemetry -d /dev/ttyS5
diff --git a/boards/x-mav/ap-h743v2/init/rc.board_extras b/boards/x-mav/ap-h743v2/init/rc.board_extras
@@ -10,4 +10,4 @@
 # fi
 
 # DShot telemetry is always on UART7
-# dshot telemetry /dev/ttyS5
+# dshot telemetry -d /dev/ttyS5
diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp
@@ -38,6 +38,7 @@
 #include <px4_platform_common/sem.hpp>
 
 char DShot::_telemetry_device[] {};
+bool DShot::_telemetry_swap_rxtx{false};
 px4::atomic_bool DShot::_request_telemetry_init{false};
 
 DShot::DShot() :
@@ -189,7 +190,7 @@ void DShot::update_num_motors()
 	_num_motors = motor_count;
 }
 
-void DShot::init_telemetry(const char *device)
+void DShot::init_telemetry(const char *device, bool swap_rxtx)
 {
 	if (!_telemetry) {
 		_telemetry = new DShotTelemetry{};
@@ -201,7 +202,7 @@ void DShot::init_telemetry(const char *device)
 	}
 
 	if (device != NULL) {
-		int ret = _telemetry->init(device);
+		int ret = _telemetry->init(device, swap_rxtx);
 
 		if (ret != 0) {
 			PX4_ERR("telemetry init failed (%i)", ret);
@@ -574,7 +575,7 @@ void DShot::Run()
 
 	// telemetry device update request?
 	if (_request_telemetry_init.load()) {
-		init_telemetry(_telemetry_device);
+		init_telemetry(_telemetry_device, _telemetry_swap_rxtx);
 		_request_telemetry_init.store(false);
 	}
 
@@ -703,33 +704,44 @@ int DShot::custom_command(int argc, char *argv[])
 {
 	const char *verb = argv[0];
 
-	if (!strcmp(verb, "telemetry")) {
-		if (argc > 1) {
-			// telemetry can be requested before the module is started
-			strncpy(_telemetry_device, argv[1], sizeof(_telemetry_device) - 1);
-			_telemetry_device[sizeof(_telemetry_device) - 1] = '\0';
-			_request_telemetry_init.store(true);
-		}
-
-		return 0;
-	}
-
 	int motor_index = -1; // select motor index, default: -1=all
 	int myoptind = 1;
+	bool swap_rxtx = false;
+	const char *device_name = nullptr;
 	int ch;
 	const char *myoptarg = nullptr;
 
-	while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "m:xd:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'm':
 			motor_index = strtol(myoptarg, nullptr, 10) - 1;
 			break;
 
+		case 'x':
+			swap_rxtx = true;
+			break;
+
+		case 'd':
+			device_name = myoptarg;
+			break;
+
 		default:
 			return print_usage("unrecognized flag");
 		}
 	}
 
+	if (!strcmp(verb, "telemetry")) {
+		if (device_name) {
+			// telemetry can be requested before the module is started
+			strncpy(_telemetry_device, device_name, sizeof(_telemetry_device) - 1);
+			_telemetry_device[sizeof(_telemetry_device) - 1] = '\0';
+			_telemetry_swap_rxtx = swap_rxtx;
+			_request_telemetry_init.store(true);
+		}
+
+		return 0;
+	}
+
 	struct VerbCommand {
 		const char *name;
 		dshot_command_t command;
@@ -844,7 +856,8 @@ After saving, the reversed direction will be regarded as the normal one. So to r
 	PRINT_MODULE_USAGE_COMMAND("start");
 
 	PRINT_MODULE_USAGE_COMMAND_DESCR("telemetry", "Enable Telemetry on a UART");
-	PRINT_MODULE_USAGE_ARG("<device>", "UART device", false);
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<device>", "UART device", false);
+	PRINT_MODULE_USAGE_PARAM_FLAG('x', "Swap RX/TX pins", true);
 
 	// DShot commands
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reverse", "Reverse motor direction");
diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h
@@ -123,7 +123,7 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 
 	void enable_dshot_outputs(const bool enabled);
 
-	void init_telemetry(const char *device);
+	void init_telemetry(const char *device, bool swap_rxtx);
 
 	int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm);
 
@@ -149,6 +149,7 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 	uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
 
 	static char _telemetry_device[20];
+	static bool _telemetry_swap_rxtx;
 	static px4::atomic_bool _request_telemetry_init;
 
 	px4::atomic<Command *> _new_command{nullptr};
diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp
@@ -49,7 +49,7 @@ DShotTelemetry::~DShotTelemetry()
 	deinit();
 }
 
-int DShotTelemetry::init(const char *uart_device)
+int DShotTelemetry::init(const char *uart_device, bool swap_rxtx)
 {
 	deinit();
 	_uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY);
@@ -59,6 +59,19 @@ int DShotTelemetry::init(const char *uart_device)
 		return -errno;
 	}
 
+	if (swap_rxtx) {
+		// Swap RX/TX pins if the device supports it
+		int rv = ioctl(_uart_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+
+		// For other devices we can still place RX on TX pin via half-duplex single-wire mode
+		if (rv) { rv = ioctl(_uart_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); }
+
+		if (rv) {
+			PX4_ERR("failed to swap rx/tx pins: %s err: %d", uart_device, rv);
+			return rv;
+		}
+	}
+
 	_num_timeouts = 0;
 	_num_successful_responses = 0;
 	_current_motor_index_request = -1;
diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h
@@ -60,7 +60,7 @@ class DShotTelemetry
 
 	~DShotTelemetry();
 
-	int init(const char *uart_device);
+	int init(const char *uart_device, bool swap_rxtx);
 
 	void deinit();
 
diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml
@@ -1,6 +1,6 @@
 module_name: DShot Driver
 serial_config:
-    - command: dshot telemetry ${SERIAL_DEV}
+    - command: dshot telemetry -d ${SERIAL_DEV}
       port_config_param:
         name: DSHOT_TEL_CFG
         group: DShot

Original file line number	Diff line number	Diff line change
`@@ -12,4 +12,4 @@ atxxxx start -s`
`12`	`12`
`13`	`13`
`14`	`14`	`# DShot telemetry is always on UART7`
`15`		`-# dshot telemetry /dev/ttyS5`
	`15`	`+# dshot telemetry -d /dev/ttyS5`