diff --git a/boards/auterion/fmu-v6s/init/rc.board_defaults b/boards/auterion/fmu-v6s/init/rc.board_defaults index b68b2b1b9861..0a568ec30ab5 100644 --- 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 index 780b13dc96b3..4611dbae462e 100644 --- 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 index 780b13dc96b3..4611dbae462e 100644 --- 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 index 5822cd29458d..33af1f8226f9 100644 --- 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 index 3423728dab9f..3629cd2958c3 100644 --- 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 index 3423728dab9f..3629cd2958c3 100644 --- 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 index 3423728dab9f..3629cd2958c3 100644 --- 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 index 3423728dab9f..3629cd2958c3 100644 --- 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 index 66e293646417..68ccc31a1da6 100644 --- 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 index bba363769ce9..61c6c0afb0c5 100644 --- 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 index bba363769ce9..61c6c0afb0c5 100644 --- 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 index 780b13dc96b3..4611dbae462e 100644 --- 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 index 10c356ade1b6..db6e16bf7d94 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -38,6 +38,7 @@ #include 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("", "UART device", false); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "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 index 9b4d8cfb484e..d0300d0470b9 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -123,7 +123,7 @@ class DShot final : public ModuleBase, 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, public OutputModuleInterface uORB::PublicationMultiData 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 _new_command{nullptr}; diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index 70992b1703ca..fe75b3685478 100644 --- 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 index 8672e85f7f48..1c5c14ef253f 100644 --- 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 index e2dcf8bc97e8..a31d394815a4 100644 --- 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