diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index 2d3e4b075b..0567759226 100644 --- a/src/drivers/dshot/DShotTelemetry.cpp +++ b/src/drivers/dshot/DShotTelemetry.cpp @@ -46,75 +46,38 @@ using namespace time_literals; DShotTelemetry::~DShotTelemetry() { - deinit(); + _uart.close(); } -int DShotTelemetry::init(const char *uart_device, bool swap_rxtx) +int DShotTelemetry::init(const char *port, bool swap_rxtx) { - int ret = OK; - deinit(); - _uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY); - - if (_uart_fd < 0) { - PX4_ERR("failed to open serial port: %s err: %d", uart_device, errno); - return -errno; + if (!_uart.setPort(port)) { + PX4_ERR("Error configuring port %s", port); + return PX4_ERROR; } - ret = setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE); - - if (ret) { - PX4_ERR("failed to set baurate: %s err: %d", uart_device, ret); - return ret; + if (!_uart.setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE)) { + PX4_ERR("Error setting baudrate on %s", port); + return PX4_ERROR; } if (swap_rxtx) { - // Swap RX/TX pins if the device supports it - ret = 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 (ret) { ret = ioctl(_uart_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); } - - if (ret) { - PX4_ERR("failed to swap rx/tx pins: %s err: %d", uart_device, ret); - return ret; + if (!_uart.setSwapRxTxMode()) { + PX4_ERR("Error swapping TX/RX"); + return PX4_ERROR; } } - _num_timeouts = 0; - _num_successful_responses = 0; - _current_motor_index_request = -1; - - return ret; -} - -void DShotTelemetry::deinit() -{ - if (_uart_fd >= 0) { - close(_uart_fd); - _uart_fd = -1; - } -} - -int DShotTelemetry::redirectOutput(OutputBuffer &buffer) -{ - if (expectingData()) { - // Error: cannot override while we already expect data - return -EBUSY; + if (! _uart.open()) { + PX4_ERR("Error opening %s", port); + return PX4_ERROR; } - _current_motor_index_request = buffer.motor_index; - _current_request_start = hrt_absolute_time(); - _redirect_output = &buffer; - _redirect_output->buf_pos = 0; - return 0; + return PX4_OK; } int DShotTelemetry::update(int num_motors) { - if (_uart_fd < 0) { - return -1; - } - if (_current_motor_index_request == -1) { // nothing in progress, start a request _current_motor_index_request = 0; @@ -124,10 +87,9 @@ int DShotTelemetry::update(int num_motors) } // read from the uart. This must be non-blocking, so check first if there is data available - int bytes_available = 0; - int ret = ioctl(_uart_fd, FIONREAD, (unsigned long)&bytes_available); + int bytes_available = _uart.bytesAvailable(); - if (ret != 0 || bytes_available <= 0) { + if (bytes_available <= 0) { // no data available. Check for a timeout const hrt_abstime now = hrt_absolute_time(); @@ -149,13 +111,12 @@ int DShotTelemetry::update(int num_motors) return -1; } - const int buf_length = ESC_FRAME_SIZE; - uint8_t buf[buf_length]; + uint8_t buf[ESC_FRAME_SIZE]; + int bytes = _uart.read(buf, sizeof(buf)); - int num_read = read(_uart_fd, buf, buf_length); - ret = -1; + int ret = -1; - for (int i = 0; i < num_read && ret == -1; ++i) { + for (int i = 0; i < bytes && ret == -1; ++i) { if (_redirect_output) { _redirect_output->buffer[_redirect_output->buf_pos++] = buf[i]; @@ -223,30 +184,27 @@ void DShotTelemetry::printStatus() const PX4_INFO("Number of CRC errors: %i", _num_checksum_errors); } -uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed) -{ - uint8_t crc_u = crc ^ crc_seed; - - for (int i = 0; i < 8; ++i) { - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - } - - return crc_u; -} - - uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len) { + auto update_crc8 = [](uint8_t crc, uint8_t crc_seed) { + uint8_t crc_u = crc ^ crc_seed; + + for (int i = 0; i < 8; ++i) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + + return crc_u; + }; + uint8_t crc = 0; for (int i = 0; i < len; ++i) { - crc = updateCrc8(buf[i], crc); + crc = update_crc8(buf[i], crc); } return crc; } - void DShotTelemetry::requestNextMotor(int num_motors) { _current_motor_index_request = (_current_motor_index_request + 1) % num_motors; @@ -418,83 +376,4 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer) PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off"); } } - - -} - -int DShotTelemetry::setBaudrate(unsigned baud) -{ - int speed; - - switch (baud) { - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; - - default: - return -EINVAL; - } - - struct termios uart_config; - - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(_uart_fd, &uart_config); - - // - // Input flags - Turn off input processing - // - // convert break to null byte, no CR to NL translation, - // no NL to CR translation, don't mark parity errors or breaks - // no input parity check, don't strip high bit off, - // no XON/XOFF software flow control - // - uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | - INLCR | PARMRK | INPCK | ISTRIP | IXON); - // - // Output flags - Turn off output processing - // - // no CR to NL translation, no NL to CR-NL translation, - // no NL to CR translation, no column 0 CR suppression, - // no Ctrl-D suppression, no fill characters, no case mapping, - // no local output processing - // - // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | - // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); - uart_config.c_oflag = 0; - - // - // No line processing - // - // echo off, echo newline off, canonical mode off, - // extended input processing off, signal chars off - // - uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - /* no parity, one stop bit, disable flow control */ - uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - return -errno; - } - - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - return -errno; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - return -errno; - } - - return 0; } diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index 1c5c14ef25..ccbe7b1ce0 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -33,6 +33,7 @@ #pragma once +#include #include class DShotTelemetry @@ -62,8 +63,6 @@ public: int init(const char *uart_device, bool swap_rxtx); - void deinit(); - /** * Read telemetry from the UART (non-blocking) and handle timeouts. * @param num_motors How many DShot enabled motors @@ -71,16 +70,6 @@ public: */ int update(int num_motors); - /** - * Redirect everything that is read into a different buffer. - * Future calls to @update will write to that instead of an internal buffer, until @update returns - * a value different from -1. No decoding is done. - * The caller must ensure the buffer exists until that point. - * @param buffer - * @return 0 on success <0 on error - */ - int redirectOutput(OutputBuffer &buffer); - bool redirectActive() const { return _redirect_output != nullptr; } /** @@ -103,13 +92,6 @@ public: private: static constexpr int ESC_FRAME_SIZE = 10; - /** - * set the Baudrate - * @param baud - * @return 0 on success, <0 on error - */ - int setBaudrate(unsigned baud); - void requestNextMotor(int num_motors); /** @@ -120,10 +102,10 @@ private: */ bool decodeByte(uint8_t byte, bool &successful_decoding); - static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed); static uint8_t crc8(const uint8_t *buf, uint8_t len); - int _uart_fd{-1}; + device::Serial _uart {}; + uint8_t _frame_buffer[ESC_FRAME_SIZE]; int _frame_position{0};