diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4bfe697a94..c6e04bbfbf 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -41,15 +41,11 @@ #include #endif -#ifndef __PX4_QURT -#include -#endif - -#include #include #include #include +#include #include #include #include @@ -165,7 +161,9 @@ public: void reset_if_scheduled(); private: - int _serial_fd{-1}; ///< serial interface to GPS + + device::Serial _serial; ///< serial interface to GPS + unsigned _baudrate{0}; ///< current baudrate const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect) char _port[20] {}; ///< device / serial port path @@ -294,16 +292,12 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]); GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, Instance instance, unsigned configured_baudrate) : Device(MODULE_NAME), + _serial(path), _configured_baudrate(configured_baudrate), _mode(mode), _interface(interface), _instance(instance) { - /* store port name */ - strncpy(_port, path, sizeof(_port) - 1); - /* enforce null termination */ - _port[sizeof(_port) - 1] = '\0'; - _report_gps_pos.heading = NAN; _report_gps_pos.heading_offset = NAN; @@ -320,7 +314,9 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac if (_interface == GPSHelper::Interface::UART) { set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL); - char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2) + const char *port = _serial.getPort(); + + char c = port[strlen(port) - 1]; // last digit of path (eg /dev/ttyS2) set_device_bus(c - 48); // sub 48 to convert char to integer } else if (_interface == GPSHelper::Interface::SPI) { @@ -398,7 +394,7 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) case GPSCallbackType::writeDeviceData: gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); - return ::write(gps->_serial_fd, data1, (size_t)data2); + return gps->_serial.write(data1, data2); case GPSCallbackType::setBaudrate: return gps->setBaudrate(data2); @@ -443,70 +439,19 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) { handleInjectDataTopic(); -#if !defined(__PX4_QURT) + // read at least 32 bytes + int timeout_us = math::min(50'000, timeout * 1000); + int ret = _serial.readAtLeast(buf, buf_length, 1, timeout_us); - /* For non QURT, use the usual polling. */ - - //Poll only for the serial data. In the same thread we also need to handle orb messages, - //so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the - //two pollings use different underlying mechanisms (at least under posix), which makes this - //impossible. Instead we limit the maximum polling interval and regularly check for new orb - //messages. - //FIXME: add a unified poll() API - const int max_timeout = 50; - - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; - - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout)); + if (ret <= 0) { + ret = _serial.read(buf, buf_length); + } if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. But don't read immediately - * by 1-2 bytes, wait for some more data to save expensive read() calls. - * If we have all requested data available, read it without waiting. - * If more bytes are available, we'll go back to poll() again. - */ - const unsigned character_count = 32; // minimum bytes that we want to read - unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate; - const unsigned sleeptime = character_count * 1000000 / (baudrate / 10); - -#ifdef __PX4_NUTTX - int err = 0; - int bytes_available = 0; - err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); - - if (err != 0 || bytes_available < (int)character_count) { - px4_usleep(sleeptime); - } - -#else - px4_usleep(sleeptime); -#endif - - ret = ::read(_serial_fd, buf, buf_length); - - if (ret > 0) { - _num_bytes_read += ret; - } - - } else { - ret = -1; - } + _num_bytes_read += ret; } return ret; - -#else - /* For QURT, just use read for now, since this doesn't block, we need to slow it down - * just a bit. */ - px4_usleep(10000); - return ::read(_serial_fd, buf, buf_length); -#endif } void GPS::handleInjectDataTopic() @@ -571,105 +516,17 @@ bool GPS::injectData(uint8_t *data, size_t len) { dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true); - size_t written = ::write(_serial_fd, data, len); - ::fsync(_serial_fd); - return written == len; + size_t written = _serial.write(data, len); + return (written == len); } int GPS::setBaudrate(unsigned baud) { - /* process baud rate */ - 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; - -#ifndef B460800 -#define B460800 460800 -#endif - - case 460800: speed = B460800; break; - -#ifndef B921600 -#define B921600 921600 -#endif - - case 921600: speed = B921600; break; - - default: - PX4_ERR("ERR: unknown baudrate: %d", baud); - return -EINVAL; + if (_serial.setBaudrate(baud)) { + return 0; } - struct termios uart_config; - - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(_serial_fd, &uart_config); - - /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ - - // - // 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) { - GPS_ERR("ERR: %d (cfsetispeed)", termios_state); - return -1; - } - - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - GPS_ERR("ERR: %d (cfsetospeed)", termios_state); - return -1; - } - - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { - GPS_ERR("ERR: %d (tcsetattr)", termios_state); - return -1; - } - - return 0; + return -1; } void GPS::initializeCommunicationDump() @@ -821,36 +678,6 @@ GPS::run() _helper = nullptr; } - if (_serial_fd < 0) { - /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); - - if (_serial_fd < 0) { - PX4_ERR("failed to open %s err: %d", _port, errno); - px4_sleep(1); - continue; - } - -#ifdef __PX4_LINUX - - if (_interface == GPSHelper::Interface::SPI) { - int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) - int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - - if (status_value < 0) { - PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } - - status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); - - if (status_value < 0) { - PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } - } - -#endif /* __PX4_LINUX */ - } - switch (_mode) { case gps_driver_mode_t::None: _mode = gps_driver_mode_t::UBX; @@ -1032,10 +859,7 @@ GPS::run() } } - if (_serial_fd >= 0) { - ::close(_serial_fd); - _serial_fd = -1; - } + _serial.close(); if (_mode_auto) { switch (_mode) { diff --git a/src/lib/drivers/device/CMakeLists.txt b/src/lib/drivers/device/CMakeLists.txt index c949db9dd5..99e876d56e 100644 --- a/src/lib/drivers/device/CMakeLists.txt +++ b/src/lib/drivers/device/CMakeLists.txt @@ -33,23 +33,49 @@ set(SRCS_PLATFORM) if (${PX4_PLATFORM} STREQUAL "nuttx") - if ("${CONFIG_I2C}" STREQUAL "y") - list(APPEND SRCS_PLATFORM nuttx/I2C.cpp) + if("${CONFIG_I2C}" STREQUAL "y") + list(APPEND SRCS_PLATFORM + nuttx/I2C.cpp + nuttx/I2C.hpp + ) endif() - if ("${CONFIG_SPI}" STREQUAL "y") - list(APPEND SRCS_PLATFORM nuttx/SPI.cpp) + if("${CONFIG_SPI}" STREQUAL "y") + list(APPEND SRCS_PLATFORM + nuttx/SPI.cpp + nuttx/SPI.hpp + ) endif() + + list(APPEND SRCS_PLATFORM + nuttx/Serial.cpp + nuttx/Serial.hpp + nuttx/SerialImpl.cpp + nuttx/SerialImpl.hpp + ) + elseif((${PX4_PLATFORM} MATCHES "qurt")) - list(APPEND SRCS_PLATFORM qurt/I2C.cpp) - list(APPEND SRCS_PLATFORM qurt/SPI.cpp) - list(APPEND SRCS_PLATFORM qurt/uart.c) + + list(APPEND SRCS_PLATFORM + qurt/I2C.cpp + ) + + list(APPEND SRCS_PLATFORM + qurt/SPI.cpp + ) + + list(APPEND SRCS_PLATFORM + qurt/uart.c + ) + elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type + # Linux I2Cdev and SPIdev list(APPEND SRCS_PLATFORM posix/I2C.cpp posix/SPI.cpp ) + endif() px4_add_library(drivers__device diff --git a/src/lib/drivers/device/nuttx/Serial.cpp b/src/lib/drivers/device/nuttx/Serial.cpp new file mode 100644 index 0000000000..7c9ec7fbd3 --- /dev/null +++ b/src/lib/drivers/device/nuttx/Serial.cpp @@ -0,0 +1,114 @@ +#include "Serial.hpp" + +namespace device +{ + +Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _impl(port, baudrate, bytesize, parity, stopbits, flowcontrol) +{ + + + // TODO: Device + + // set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL); + + // char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2) + // set_device_bus(c - 48); // sub 48 to convert char to integer +} + +Serial::~Serial() +{ +} + +bool Serial::open() +{ + return _impl.open(); +} + +bool Serial::isOpen() const +{ + return _impl.isOpen(); +} + +bool Serial::close() +{ + return _impl.close(); +} + +ssize_t Serial::read(uint8_t *buffer, size_t buffer_size) +{ + return _impl.read(buffer, buffer_size); +} + +ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us); +} + +ssize_t Serial::write(const void *buffer, size_t buffer_size) +{ + return _impl.write(buffer, buffer_size); +} + +const char *Serial::getPort() const +{ + return _impl.getPort(); +} + +bool Serial::setPort(const char *port) +{ + return _impl.setPort(port); +} + +uint32_t Serial::getBaudrate() const +{ + return _impl.getBaudrate(); +} + +bool Serial::setBaudrate(uint32_t baudrate) +{ + return _impl.setBaudrate(baudrate); +} + +ByteSize Serial::getBytesize() const +{ + return _impl.getBytesize(); +} + +void Serial::setBytesize(ByteSize bytesize) +{ + _impl.setBytesize(bytesize); +} + +Parity Serial::getParity() const +{ + return _impl.getParity(); +} + +void Serial::setParity(Parity parity) +{ + _impl.setParity(parity); +} + +StopBits Serial::getStopbits() const +{ + return _impl.getStopbits(); +} + +void Serial::setStopbits(StopBits stopbits) +{ + _impl.setStopbits(stopbits); +} + +FlowControl Serial::getFlowcontrol() const +{ + return _impl.getFlowcontrol(); +} + +void Serial::setFlowcontrol(FlowControl flowcontrol) +{ + _impl.setFlowcontrol(flowcontrol); +} + +} // namespace device diff --git a/src/lib/drivers/device/nuttx/Serial.hpp b/src/lib/drivers/device/nuttx/Serial.hpp new file mode 100644 index 0000000000..b2f50163bd --- /dev/null +++ b/src/lib/drivers/device/nuttx/Serial.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "SerialImpl.hpp" + +#include "SerialCommon.hpp" + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device __EXPORT +{ + +class Serial +{ +public: + Serial(const char *port, uint32_t baudrate = 57600, + ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None, + StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled); + virtual ~Serial(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + const char *getPort() const; + bool setPort(const char *port); + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + void setBytesize(ByteSize bytesize); + + Parity getParity() const; + void setParity(Parity parity); + + StopBits getStopbits() const; + void setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + void setFlowcontrol(FlowControl flowcontrol); + + // printStatus() + // port, read bytes, write bytes + +private: + // Disable copy constructors + Serial(const Serial &); + Serial &operator=(const Serial &); + + // platform implementation + SerialImpl _impl; +}; + +} // namespace device diff --git a/src/lib/drivers/device/nuttx/SerialCommon.hpp b/src/lib/drivers/device/nuttx/SerialCommon.hpp new file mode 100644 index 0000000000..9c36a21b14 --- /dev/null +++ b/src/lib/drivers/device/nuttx/SerialCommon.hpp @@ -0,0 +1,38 @@ + +#pragma once + +namespace device +{ +namespace SerialConfig +{ + + +// ByteSize: number of data bits +enum class ByteSize { + FiveBits = 5, + SixBits = 6, + SevenBits = 7, + EightBits = 8, +}; + +// Parity: enable parity checking +enum class Parity { + None = 0, + Odd = 1, + Even = 2, +}; + +// StopBits: number of stop bits +enum class StopBits { + One = 1, + Two = 2 +}; + +// FlowControl: enable flow control +enum class FlowControl { + Disabled = 0, + Enabled = 1, +}; + +} // namespace Serial +} // namespace device diff --git a/src/lib/drivers/device/nuttx/SerialImpl.cpp b/src/lib/drivers/device/nuttx/SerialImpl.cpp new file mode 100644 index 0000000000..269ea19a9a --- /dev/null +++ b/src/lib/drivers/device/nuttx/SerialImpl.cpp @@ -0,0 +1,523 @@ + +#include "SerialImpl.hpp" + +#include +#include // open +#include +#include // strncpy +#include + +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + } + + setBaudrate(baudrate); +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::open() +{ + // if already open first close + if (isOpen()) { + return true; + } + + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s, errno: %d, %s", _port, errno, strerror(errno)); + return false; + } + + _serial_fd = serial_fd; + _open = true; + + if (configure()) { + _configured = true; + } + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + int ret = 0; + + if (_serial_fd >= 0) { + ret = ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + _configured = false; + + return (ret == 0); +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + open(); + } + + if (!_configured) { + configure(); + } + + int ret_read = ::read(_serial_fd, buffer, buffer_size); + + if (ret_read == -1) { + switch (errno) { + case EBADF: + // invalid file descriptor + PX4_ERR("%s read error %d %s", _port, errno, strerror(errno)); + close(); + break; + } + + PX4_DEBUG("%s read error %d %s", _port, errno, strerror(errno)); + + } else { + _bytes_read += ret_read; + } + + return ret_read; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + const size_t required_bytes = math::min(buffer_size, character_count); + const hrt_abstime start_time_us = hrt_absolute_time(); + + if (!_open) { + open(); + } + + if (!_configured) { + configure(); + } + + while (true) { + int bytes_available = 0; + int ioctl_ret = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + + if ((ioctl_ret == 0) && (bytes_available >= (int)required_bytes)) { + return read(buffer, buffer_size); + } + + if (bytes_available < (int)required_bytes) { + + if (timeout_us > 0) { + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + + if (elapsed_us > timeout_us) { + //PX4_WARN("readAtLeast timeout %d bytes available (%llu us elapsed)", bytes_available, elapsed_us); + return -1; + } + } + + int desired_bytes = required_bytes - bytes_available; + + uint32_t sleeptime_us = desired_bytes * 1'000'000 / (_baudrate / 10); + + if (desired_bytes == 1 || sleeptime_us <= 1000) { + + int poll_timeout_ms = 0; + + if (timeout_us > 0) { + poll_timeout_ms = timeout_us / 1000; + } + + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + int poll_ret = ::poll(fds, 1, poll_timeout_ms); + + if (poll_ret > 0) { + if (fds[0].revents & POLLIN) { + // There is data to read + } + } + + } else { + if (timeout_us > 0) { + sleeptime_us = math::min(sleeptime_us, timeout_us); + } + + //PX4_INFO("%s %d/%d bytes available, sleep time %" PRIu32 "us", _port, bytes_available, required_bytes, sleeptime_us); + + px4_usleep(sleeptime_us); + } + } + } + + return -1; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + open(); + } + + if (!_configured) { + configure(); + } + + // POLLOUT: Writing is now possible + + // FIONWRITE: Get the number of bytes that have been written to the TX buffer. + // FIONSPACE: Get the number of free bytes in the TX buffer + + int ret_write = ::write(_serial_fd, buffer, buffer_size); + + if (ret_write == -1) { + + switch (errno) { + case EBADF: + // invalid file descriptor + close(); + break; + } + + PX4_ERR("%s write error %d %s", _port, errno, strerror(errno)); + + } else { + _bytes_written += ret_write; + } + + return ret_write; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +bool SerialImpl::setPort(const char *port) +{ + if (port) { + + if (strcmp(port, _port) == 0) { + return true; + } + + if (::access(port, R_OK) != 0) { + PX4_ERR("port %s not accessible", port); + return false; + } + + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + _configured = false; + + return true; + } + + return false; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + // process baud rate change + int speed = 0; + + switch (baudrate) { + case 4800: speed = B4800; break; + + 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; + +#if defined(B460800) + + case 460800: speed = B460800; break; +#endif // B460800 + +#if defined(B500000) + + case 500000: speed = B500000; break; +#endif // B500000 + +#if defined(B576000) + + case 576000: speed = B576000; break; +#endif // B576000 + +#if defined(B921600) + + case 921600: speed = B921600; break; +#endif // B921600 + +#if defined(B1000000) + + case 1000000: speed = B1000000; break; +#endif // B1000000 + +#if defined(B1500000) + + case 1500000: speed = B1500000; break; +#endif // B1500000 + +#if defined(B2000000) + + case 2000000: speed = B2000000; break; +#endif // B2000000 + +#if defined(B2500000) + + case 2500000: speed = B2500000; break; +#endif // B2500000 + +#if defined(B3000000) + + case 3000000: speed = B3000000; break; +#endif // B3000000 + +#if defined(B3500000) + + case 3500000: speed = B3500000; break; +#endif // B3500000 + +#if defined(B4000000) + + case 4000000: speed = B4000000; break; +#endif // B4000000 + + default: + PX4_ERR("unknown baudrate: %" PRIu32, baudrate); + return false; + } + + // check if already configured + if (speed == (int)_baudrate) { + return true; + } + + if (speed != 0) { + _baudrate = speed; + _configured = false; + return true; + } + + return false; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +void SerialImpl::setBytesize(ByteSize bytesize) +{ + if (bytesize != _bytesize) { + _bytesize = bytesize; + _configured = false; + } +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +void SerialImpl::setParity(Parity parity) +{ + if (parity != _parity) { + _parity = parity; + _configured = false; + } +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +void SerialImpl::setStopbits(StopBits stopbits) +{ + if (stopbits != _stopbits) { + _stopbits = stopbits; + _configured = false; + } +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +void SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + if (flowcontrol != _flowcontrol) { + _flowcontrol = flowcontrol; + _configured = false; + } +} + +bool SerialImpl::configure() +{ + _configured = false; + + if (!isOpen()) { + return false; + } + + // fill the struct for the new configuration + struct termios uart_config; + + if (tcgetattr(_serial_fd, &uart_config) != 0) { + PX4_ERR("tcgetattr failed"); + return false; + } + + // Input modes (c_iflag): Turn off input processing + uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); + uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off software flow ctrl + + // Output modes (c_oflag): Turn off output processing + uart_config.c_oflag = 0; + uart_config.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars) + uart_config.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed + + // Local modes (c_lflag): No line processing + uart_config.c_lflag &= ~(ECHO | ECHOE | ECHONL | ICANON | ISIG | IEXTEN); + + // Control Modes (c_cflag) + uart_config.c_cflag = CREAD | CLOCAL; + + + // ByteSize: termios CSIZE + uart_config.c_cflag &= ~CSIZE; // Clear all the size bits, then use one of the statements below + + switch (_bytesize) { + case ByteSize::FiveBits: + uart_config.c_cflag |= CS5; + break; + + case ByteSize::SixBits: + uart_config.c_cflag |= CS6; + break; + + case ByteSize::SevenBits: + uart_config.c_cflag |= CS7; + break; + + case ByteSize::EightBits: + uart_config.c_cflag |= CS8; + break; + } + + // StopBits: termios CSTOPB + switch (_stopbits) { + case StopBits::One: + uart_config.c_cflag &= ~CSTOPB; + break; + + case StopBits::Two: + uart_config.c_cflag |= CSTOPB; + break; + } + + // Parity: termios PARENB, PARODD + switch (_parity) { + case Parity::None: + uart_config.c_cflag &= ~(PARENB | PARODD); + break; + + case Parity::Odd: + uart_config.c_cflag |= (PARENB | PARODD); + break; + + case Parity::Even: + uart_config.c_cflag |= PARENB; + uart_config.c_cflag &= ~PARODD; + break; + } + + // FlowControl: termios CRTSCTS + switch (_flowcontrol) { + case FlowControl::Disabled: + uart_config.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control + break; + + case FlowControl::Enabled: + uart_config.c_cflag |= CRTSCTS; // Enable RTS/CTS hardware flow control + break; + } + + // VMIN = 0, VTIME = 0: No blocking, return immediately with what is available + uart_config.c_cc[VMIN] = 0; + uart_config.c_cc[VTIME] = 0; + + + // baudrate: input speed (cfsetispeed) + int cfsetispeed_ret = cfsetispeed(&uart_config, _baudrate); + + if (cfsetispeed_ret != 0) { + PX4_ERR("cfsetispeed failed: %d (errno=%d, %s)", cfsetispeed_ret, errno, strerror(errno)); + return false; + } + + // baudrate: output speed (cfsetospeed) + int cfsetospeed_ret = cfsetospeed(&uart_config, _baudrate); + + if (cfsetospeed_ret != 0) { + PX4_ERR("cfsetospeed failed: %d (errno=%d, %s)", cfsetospeed_ret, errno, strerror(errno)); + return false; + } + + // tcsetattr: set attributes + int tcsetattr_ret = tcsetattr(_serial_fd, TCSANOW, &uart_config); + + if (tcsetattr_ret != 0) { + PX4_ERR("tcsetattr failed: %d (errno=%d, %s)", tcsetattr_ret, errno, strerror(errno)); + return false; + } + + PX4_INFO("%s configured successfully", _port); + _configured = true; + return true; +} + +} // namespace device diff --git a/src/lib/drivers/device/nuttx/SerialImpl.hpp b/src/lib/drivers/device/nuttx/SerialImpl.hpp new file mode 100644 index 0000000000..4e0330a5b9 --- /dev/null +++ b/src/lib/drivers/device/nuttx/SerialImpl.hpp @@ -0,0 +1,82 @@ + + +#pragma once + +#include + +#include "SerialCommon.hpp" + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + const char *getPort() const; + bool setPort(const char *port); + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + void setBytesize(ByteSize bytesize); + + Parity getParity() const; + void setParity(Parity parity); + + StopBits getStopbits() const; + void setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + void setFlowcontrol(FlowControl flowcontrol); + +private: + + bool configure(); + + + int _serial_fd{-1}; + + size_t _bytes_read{0}; + size_t _bytes_written{0}; + + bool _open{false}; + bool _configured{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + // Mutex used to lock the read functions + //pthread_mutex_t read_mutex; + + // Mutex used to lock the write functions + //pthread_mutex_t write_mutex; +}; + +} // namespace device