diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index abba5df26d..689fcd1c8b 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -10,6 +10,7 @@ CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y +CONFIG_DRIVERS_RC_CRSF_RC=y CONFIG_DRIVERS_QSHELL_QURT=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y diff --git a/src/drivers/rc/crsf_rc/CMakeLists.txt b/src/drivers/rc/crsf_rc/CMakeLists.txt index ceb7531079..a3ebb1a1ae 100644 --- a/src/drivers/rc/crsf_rc/CMakeLists.txt +++ b/src/drivers/rc/crsf_rc/CMakeLists.txt @@ -46,6 +46,4 @@ px4_add_module( MODULE_CONFIG module.yaml - DEPENDS - rc ) diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index dc30620e98..23389c5789 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -35,8 +35,6 @@ #include "CrsfParser.hpp" #include "Crc8.hpp" -#include -#include #include #include @@ -118,76 +116,76 @@ void CrsfRc::Run() { if (should_exit()) { ScheduleClear(); - ::close(_rc_fd); - _rc_fd = -1; + if (_uart) { + (void) _uart->close(); + delete _uart; + _uart = nullptr; + } exit_and_cleanup(); return; } - if (_rc_fd < 0) { - _rc_fd = ::open(_device, O_RDWR | O_NONBLOCK); + if (_uart == nullptr) { + // Create the UART port instance + _uart = new Serial(_device); - if (_rc_fd >= 0) { - struct termios t; - - tcgetattr(_rc_fd, &t); - cfsetspeed(&t, CRSF_BAUDRATE); - t.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); - t.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - t.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); - t.c_oflag = 0; - tcsetattr(_rc_fd, TCSANOW, &t); - - if (board_rc_swap_rxtx(_device)) { -#if defined(TIOCSSWAP) - ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED); -#endif // TIOCSSWAP - } - - if (board_rc_singlewire(_device)) { - _is_singlewire = true; -#if defined(TIOCSSINGLEWIRE) - ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); -#endif // TIOCSSINGLEWIRE - } - - PX4_INFO("Crsf serial opened sucessfully"); - - if (_is_singlewire) { - PX4_INFO("Crsf serial is single wire. Telemetry disabled"); - } - - tcflush(_rc_fd, TCIOFLUSH); - - Crc8Init(0xd5); + if (_uart == nullptr) { + PX4_ERR("Error creating serial device %s", _device); + px4_sleep(1); + return; } + } + + 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); + px4_sleep(1); + return; + } + + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart->open()) { + PX4_ERR("Error opening serial device %s", _device); + px4_sleep(1); + return; + } + +// if (board_rc_swap_rxtx(_device)) { +// #if defined(TIOCSSWAP) +// ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED); +// #endif // TIOCSSWAP +// } +// +// if (board_rc_singlewire(_device)) { +// _is_singlewire = true; +// #if defined(TIOCSSINGLEWIRE) +// ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +// #endif // TIOCSSINGLEWIRE +// } +// +// PX4_INFO("Crsf serial opened sucessfully"); +// +// if (_is_singlewire) { +// PX4_INFO("Crsf serial is single wire. Telemetry disabled"); +// } +// +// tcflush(_rc_fd, TCIOFLUSH); + + Crc8Init(0xd5); _input_rc.rssi_dbm = NAN; _input_rc.link_quality = -1; CrsfParser_Init(); - - - } - - // poll with 100mS timeout - pollfd fds[1]; - fds[0].fd = _rc_fd; - fds[0].events = POLLIN; - int ret = ::poll(fds, 1, 100); - - if (ret < 0) { - PX4_ERR("poll error"); - // try again with delay - ScheduleDelayed(100_ms); - return; } const hrt_abstime time_now_us = hrt_absolute_time(); perf_count_interval(_cycle_interval_perf, time_now_us); // Read all available data from the serial RC input UART - int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE); + int new_bytes = _uart->readAtLeast(&_rcs_buf[0], RC_MAX_BUFFER_SIZE, 1, 100); if (new_bytes > 0) { _bytes_rx += new_bytes; @@ -433,7 +431,8 @@ bool CrsfRc::SendTelemetryBattery(const uint16_t voltage, const uint16_t current write_uint24_t(buf, offset, fuel); write_uint8_t(buf, offset, remaining); WriteFrameCrc(buf, offset, sizeof(buf)); - return write(_rc_fd, buf, offset) == offset; + return _uart->write((void *) buf, (size_t) offset); + } bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed, @@ -449,7 +448,7 @@ bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, c write_uint16_t(buf, offset, altitude); write_uint8_t(buf, offset, num_satellites); WriteFrameCrc(buf, offset, sizeof(buf)); - return write(_rc_fd, buf, offset) == offset; + return _uart->write((void *) buf, (size_t) offset); } bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw) @@ -461,7 +460,7 @@ bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, cons write_uint16_t(buf, offset, roll); write_uint16_t(buf, offset, yaw); WriteFrameCrc(buf, offset, sizeof(buf)); - return write(_rc_fd, buf, offset) == offset; + return _uart->write((void *) buf, (size_t) offset); } bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode) @@ -480,7 +479,7 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode) offset += length; buf[offset - 1] = 0; // ensure null-terminated string WriteFrameCrc(buf, offset, length + 4); - return write(_rc_fd, buf, offset) == offset; + return _uart->write((void *) buf, (size_t) offset); } int CrsfRc::print_status() diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index 6603e01ea3..c3b0a4ec54 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.hpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -53,6 +54,8 @@ #include #include +using namespace device; + class CrsfRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -87,7 +90,8 @@ private: bool SendTelemetryFlightMode(const char *flight_mode); - int _rc_fd{-1}; + Serial *_uart = nullptr; ///< UART interface to RC + char _device[20] {}; ///< device / serial port path bool _is_singlewire{false};