diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index f099fff909..069be2586c 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -36,6 +36,7 @@ #include "Crc8.hpp" #include +#include #include #include @@ -44,11 +45,10 @@ using namespace time_literals; -#define CRSF_BAUDRATE 420000 - -CrsfRc::CrsfRc(const char *device) : +CrsfRc::CrsfRc(const char *device, uint32_t baudrate) : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)) + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), + _baudrate(baudrate) { if (device) { strncpy(_device, device, sizeof(_device) - 1); @@ -70,13 +70,18 @@ int CrsfRc::task_spawn(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; const char *device_name = nullptr; + uint32_t baudrate = 420'000; - while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': device_name = myoptarg; break; + case 'b': + baudrate = strtoul(myoptarg, nullptr, 10); + break; + case '?': error_flag = true; break; @@ -102,7 +107,7 @@ int CrsfRc::task_spawn(int argc, char *argv[]) return PX4_ERROR; } - CrsfRc *instance = new CrsfRc(device_name); + CrsfRc *instance = new CrsfRc(device_name, baudrate); if (instance == nullptr) { PX4_ERR("alloc failed"); @@ -144,10 +149,9 @@ void CrsfRc::Run() } if (! _uart->isOpen()) { - // Configure the desired baudrate if one was specified by the user. - // Otherwise the default baudrate will be used. - if (! _uart->setBaudrate(CRSF_BAUDRATE)) { - PX4_ERR("Error setting baudrate to %u on %s", CRSF_BAUDRATE, _device); + // Configure the UART. + if (_baudrate && ! _uart->setBaudrate(_baudrate)) { + PX4_ERR("Error setting baudrate to %" PRIu32 " on %s", _baudrate, _device); px4_sleep(1); return; } @@ -565,7 +569,7 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem PRINT_MODULE_USAGE_SUBCATEGORY("radio_control"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); - + PRINT_MODULE_USAGE_PARAM_INT('b', 420000, 4800, 3000000, "RC baudrate", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index a47b361903..84d78348af 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.hpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.hpp @@ -59,7 +59,7 @@ using namespace device; class CrsfRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - CrsfRc(const char *device); + CrsfRc(const char *device, uint32_t baudrate); ~CrsfRc() override; /** @see ModuleBase */ @@ -94,6 +94,7 @@ private: char _device[20] {}; ///< device / serial port path bool _is_singlewire{false}; + uint32_t _baudrate{0}; static constexpr size_t RC_MAX_BUFFER_SIZE{64}; uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; diff --git a/src/drivers/rc/crsf_rc/module.yaml b/src/drivers/rc/crsf_rc/module.yaml index fc4ce23a27..4e76ea5976 100644 --- a/src/drivers/rc/crsf_rc/module.yaml +++ b/src/drivers/rc/crsf_rc/module.yaml @@ -1,6 +1,6 @@ module_name: CRSF RC Input Driver serial_config: - - command: "crsf_rc start -d ${SERIAL_DEV}" + - command: "crsf_rc start -d ${SERIAL_DEV} -b ${BAUD_PARAM}" port_config_param: name: RC_CRSF_PRT_CFG group: Serial