From baa857d56f1fb2bfb560f6da7816a32f2d2765a1 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Wed, 27 Dec 2023 13:08:26 -0800 Subject: [PATCH] Added a new device driver for a platform independent UART implementation --- src/lib/drivers/device/CMakeLists.txt | 5 + src/lib/drivers/device/Serial.cpp | 138 +++++++ src/lib/drivers/device/Serial.hpp | 103 ++++++ src/lib/drivers/device/SerialCommon.hpp | 70 ++++ src/lib/drivers/device/nuttx/SerialImpl.cpp | 382 +++++++++++++++++++ src/lib/drivers/device/nuttx/SerialImpl.hpp | 104 ++++++ src/lib/drivers/device/posix/SerialImpl.cpp | 384 ++++++++++++++++++++ src/lib/drivers/device/posix/SerialImpl.hpp | 103 ++++++ src/lib/drivers/device/qurt/SerialImpl.cpp | 326 +++++++++++++++++ src/lib/drivers/device/qurt/SerialImpl.hpp | 108 ++++++ 10 files changed, 1723 insertions(+) create mode 100644 src/lib/drivers/device/Serial.cpp create mode 100644 src/lib/drivers/device/Serial.hpp create mode 100644 src/lib/drivers/device/SerialCommon.hpp create mode 100644 src/lib/drivers/device/nuttx/SerialImpl.cpp create mode 100644 src/lib/drivers/device/nuttx/SerialImpl.hpp create mode 100644 src/lib/drivers/device/posix/SerialImpl.cpp create mode 100644 src/lib/drivers/device/posix/SerialImpl.hpp create mode 100644 src/lib/drivers/device/qurt/SerialImpl.cpp create mode 100644 src/lib/drivers/device/qurt/SerialImpl.hpp diff --git a/src/lib/drivers/device/CMakeLists.txt b/src/lib/drivers/device/CMakeLists.txt index c949db9dd5..3d3eab11d1 100644 --- a/src/lib/drivers/device/CMakeLists.txt +++ b/src/lib/drivers/device/CMakeLists.txt @@ -40,20 +40,25 @@ if (${PX4_PLATFORM} STREQUAL "nuttx") if ("${CONFIG_SPI}" STREQUAL "y") list(APPEND SRCS_PLATFORM nuttx/SPI.cpp) endif() + + list(APPEND SRCS_PLATFORM nuttx/SerialImpl.cpp) elseif((${PX4_PLATFORM} MATCHES "qurt")) list(APPEND SRCS_PLATFORM qurt/I2C.cpp) list(APPEND SRCS_PLATFORM qurt/SPI.cpp) + list(APPEND SRCS_PLATFORM qurt/SerialImpl.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 + posix/SerialImpl.cpp ) endif() px4_add_library(drivers__device CDev.cpp + Serial.cpp ${SRCS_PLATFORM} ) diff --git a/src/lib/drivers/device/Serial.cpp b/src/lib/drivers/device/Serial.cpp new file mode 100644 index 0000000000..3d4a3474f3 --- /dev/null +++ b/src/lib/drivers/device/Serial.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +#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) +{ + // If no baudrate was specified then set it to a reasonable default value + if (baudrate == 0) { + (void) _impl.setBaudrate(9600); + } +} + +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); +} + +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(); +} + +bool Serial::setBytesize(ByteSize bytesize) +{ + return _impl.setBytesize(bytesize); +} + +Parity Serial::getParity() const +{ + return _impl.getParity(); +} + +bool Serial::setParity(Parity parity) +{ + return _impl.setParity(parity); +} + +StopBits Serial::getStopbits() const +{ + return _impl.getStopbits(); +} + +bool Serial::setStopbits(StopBits stopbits) +{ + return _impl.setStopbits(stopbits); +} + +FlowControl Serial::getFlowcontrol() const +{ + return _impl.getFlowcontrol(); +} + +bool Serial::setFlowcontrol(FlowControl flowcontrol) +{ + return _impl.setFlowcontrol(flowcontrol); +} + +const char *Serial::getPort() const +{ + return _impl.getPort(); +} + +} // namespace device diff --git a/src/lib/drivers/device/Serial.hpp b/src/lib/drivers/device/Serial.hpp new file mode 100644 index 0000000000..aa05b0160f --- /dev/null +++ b/src/lib/drivers/device/Serial.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 + +#ifdef __PX4_NUTTX +#include "nuttx/SerialImpl.hpp" +#elif defined(__PX4_QURT) +#include "qurt/SerialImpl.hpp" +#else +#include "posix/SerialImpl.hpp" +#endif + +#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(); + + // Open sets up the port and gets it configured based on desired configuration + 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); + + // If port is already open then the following configuration functions + // will reconfigure the port. If the port is not yet open then they will + // simply store the configuration in preparation for the port to be opened. + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + + const char *getPort() const; + +private: + // Disable copy constructors + Serial(const Serial &); + Serial &operator=(const Serial &); + + // platform implementation + SerialImpl _impl; +}; + +} // namespace device diff --git a/src/lib/drivers/device/SerialCommon.hpp b/src/lib/drivers/device/SerialCommon.hpp new file mode 100644 index 0000000000..bbe9be0ad9 --- /dev/null +++ b/src/lib/drivers/device/SerialCommon.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 + +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 SerialConfig +} // 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..42588ed413 --- /dev/null +++ b/src/lib/drivers/device/nuttx/SerialImpl.cpp @@ -0,0 +1,382 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +#include "SerialImpl.hpp" +#include // strncpy +#include +#include +#include +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); +} + +bool SerialImpl::configure() +{ + /* process baud rate */ + int speed; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("ERR: unknown baudrate: %lu", _baudrate); + return false; + } + + switch (_baudrate) { + 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: %lu", _baudrate); + return false; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcgetattr)", termios_state); + return false; + } + + /* 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) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return false; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return false; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + if (isOpen()) { + return true; + } + + // Open the serial port + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + return false; + } + + _serial_fd = serial_fd; + + // Configure the serial port + if (! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + + if (_serial_fd >= 0) { + ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret = ::read(_serial_fd, buffer, buffer_size); + + if (ret < 0) { + PX4_DEBUG("%s read error %d", _port, ret); + } + + return ret; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) { + // Poll for incoming UART data. + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); + + if (remaining_time <= 0) { break; } + + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); + + if (ret > 0) { + if (fds[0].revents & POLLIN) { + ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (ret > 0) { + total_bytes_read += ret; + } + + } else { + PX4_ERR("Got a poll error"); + return -1; + } + } + } + + return total_bytes_read; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int written = ::write(_serial_fd, buffer, buffer_size); + ::fsync(_serial_fd); + + if (written < 0) { + PX4_ERR("%s write error %d", _port, written); + } + + return written; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("ERR: invalid baudrate: %lu", baudrate); + return false; + } + + // check if already configured + if ((baudrate == _baudrate) && _open) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return configure(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // 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..799fe54463 --- /dev/null +++ b/src/lib/drivers/device/nuttx/SerialImpl.hpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include + +#include + +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; + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + bool configure(); + +}; + +} // namespace device diff --git a/src/lib/drivers/device/posix/SerialImpl.cpp b/src/lib/drivers/device/posix/SerialImpl.cpp new file mode 100644 index 0000000000..c216e32656 --- /dev/null +++ b/src/lib/drivers/device/posix/SerialImpl.cpp @@ -0,0 +1,384 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); +} + +bool SerialImpl::configure() +{ + /* process baud rate */ + int speed; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("ERR: unknown baudrate: %u", _baudrate); + return false; + } + + switch (_baudrate) { + 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", _baudrate); + return false; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcgetattr)", termios_state); + return false; + } + + /* 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) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return false; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return false; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + if (isOpen()) { + return true; + } + + // Open the serial port + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + return false; + } + + _serial_fd = serial_fd; + + // Configure the serial port + if (! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + + if (_serial_fd >= 0) { + ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret = ::read(_serial_fd, buffer, buffer_size); + + if (ret < 0) { + PX4_DEBUG("%s read error %d", _port, ret); + + } + + return ret; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) { + // Poll for incoming UART data. + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); + + if (remaining_time <= 0) { break; } + + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); + + if (ret > 0) { + if (fds[0].revents & POLLIN) { + ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (ret > 0) { + total_bytes_read += ret; + } + + } else { + PX4_ERR("Got a poll error"); + return -1; + } + } + } + + return total_bytes_read; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int written = ::write(_serial_fd, buffer, buffer_size); + ::fsync(_serial_fd); + + if (written < 0) { + PX4_ERR("%s write error %d", _port, written); + + } + + return written; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("ERR: invalid baudrate: %u", baudrate); + return false; + } + + // check if already configured + if ((baudrate == _baudrate) && _open) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return configure(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // namespace device diff --git a/src/lib/drivers/device/posix/SerialImpl.hpp b/src/lib/drivers/device/posix/SerialImpl.hpp new file mode 100644 index 0000000000..efc95d7d51 --- /dev/null +++ b/src/lib/drivers/device/posix/SerialImpl.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include + +#include + +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; + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + bool configure(); +}; + +} // namespace device diff --git a/src/lib/drivers/device/qurt/SerialImpl.cpp b/src/lib/drivers/device/qurt/SerialImpl.cpp new file mode 100644 index 0000000000..ec0fb73fce --- /dev/null +++ b/src/lib/drivers/device/qurt/SerialImpl.cpp @@ -0,0 +1,326 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + if ((baudrate != 9600) && + (baudrate != 38400) && + (baudrate != 57600) && + (baudrate != 115200) && + (baudrate != 230400) && + (baudrate != 250000) && + (baudrate != 420000) && + (baudrate != 460800) && + (baudrate != 921600) && + (baudrate != 1000000) && + (baudrate != 1843200) && + (baudrate != 2000000)) { + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + // There's no harm in calling open multiple times on the same port. + // In fact, that's the only way to change the baudrate + + _open = false; + _serial_fd = -1; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("Invalid baudrate: %u", _baudrate); + return false; + } + + if (_bytesize != ByteSize::EightBits) { + PX4_ERR("Qurt platform only supports ByteSize::EightBits"); + return false; + } + + if (_parity != Parity::None) { + PX4_ERR("Qurt platform only supports Parity::None"); + return false; + } + + if (_stopbits != StopBits::One) { + PX4_ERR("Qurt platform only supports StopBits::One"); + return false; + } + + if (_flowcontrol != FlowControl::Disabled) { + PX4_ERR("Qurt platform only supports FlowControl::Disabled"); + return false; + } + + // qurt_uart_open will check validity of port and baudrate + int serial_fd = qurt_uart_open(_port, _baudrate); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd); + return false; + + } else { + PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate); + } + + _serial_fd = serial_fd; + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + // No close defined for qurt uart yet + // if (_serial_fd >= 0) { + // qurt_uart_close(_serial_fd); + // } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500); + + if (ret_read < 0) { + PX4_DEBUG("%s read error %d", _port, ret_read); + + } + + return ret_read; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while (total_bytes_read < (int) character_count) { + + if (timeout_us > 0) { + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + + if (elapsed_us >= timeout_us) { + // If there was a partial read but not enough to satisfy the minimum then they will be lost + // but this really should never happen when everything is working normally. + // PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us); + // Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)? + return total_bytes_read; + } + } + + int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (current_bytes_read < 0) { + // Again, if there was a partial read but not enough to satisfy the minimum then they will be lost + // but this really should never happen when everything is working normally. + PX4_ERR("%s failed to read uart", __FUNCTION__); + // Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)? + return -1; + } + + // Current bytes read could be zero + total_bytes_read += current_bytes_read; + + // If we have at least reached our desired minimum number of characters + // then we can return now + if (total_bytes_read >= (int) character_count) { + return total_bytes_read; + } + + // Wait a set amount of time before trying again or the remaining time + // until the timeout if we are getting close + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + int64_t time_until_timeout = timeout_us - elapsed_us; + uint64_t time_to_sleep = 5000; + + if ((time_until_timeout >= 0) && + (time_until_timeout < (int64_t) time_to_sleep)) { + time_to_sleep = time_until_timeout; + } + + px4_usleep(time_to_sleep); + } + + return -1; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size); + + if (ret_write < 0) { + PX4_ERR("%s write error %d", _port, ret_write); + + } + + return ret_write; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("Invalid baudrate: %u", baudrate); + return false; + } + + // check if already configured + if (baudrate == _baudrate) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return open(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // namespace device diff --git a/src/lib/drivers/device/qurt/SerialImpl.hpp b/src/lib/drivers/device/qurt/SerialImpl.hpp new file mode 100644 index 0000000000..1b98d3bb40 --- /dev/null +++ b/src/lib/drivers/device/qurt/SerialImpl.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 + +#include + +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; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + + // 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