From fbfb93ab2654e8aceeea9a1f597e1ab90cd5e5c3 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 28 Dec 2023 17:33:18 -0800 Subject: [PATCH] Cleaned up the API and added SBUS support --- src/drivers/gps/gps.cpp | 6 +- src/lib/drivers/device/CMakeLists.txt | 2 + src/lib/drivers/device/Serial.cpp | 47 +--- src/lib/drivers/device/Serial.hpp | 38 +-- src/lib/drivers/device/nuttx/SerialImpl.cpp | 241 ++++++++---------- src/lib/drivers/device/nuttx/SerialImpl.hpp | 31 +-- src/lib/drivers/device/posix/SerialImpl.cpp | 191 +++----------- src/lib/drivers/device/posix/SerialImpl.hpp | 36 +-- .../drivers/device/posix/SerialSBUSImpl.cpp | 80 ++++++ .../SerialSBUSImpl.hpp} | 37 +-- .../device/posix/SerialStandardImpl.cpp | 159 ++++++++++++ .../device/posix/SerialStandardImpl.hpp | 55 ++++ src/lib/drivers/device/qurt/SerialImpl.cpp | 109 +------- src/lib/drivers/device/qurt/SerialImpl.hpp | 38 +-- 14 files changed, 500 insertions(+), 570 deletions(-) create mode 100644 src/lib/drivers/device/posix/SerialSBUSImpl.cpp rename src/lib/drivers/device/{SerialCommon.hpp => posix/SerialSBUSImpl.hpp} (80%) create mode 100644 src/lib/drivers/device/posix/SerialStandardImpl.cpp create mode 100644 src/lib/drivers/device/posix/SerialStandardImpl.hpp diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 59db88d7c5..0fcd572ef6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -791,7 +791,7 @@ GPS::run() if (_uart == nullptr) { PX4_ERR("Error creating serial device %s", _port); - px4_sleep(1); + px4_usleep(100000); continue; } } @@ -802,7 +802,7 @@ GPS::run() if (_configured_baudrate) { if (! _uart->setBaudrate(_configured_baudrate)) { PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port); - px4_sleep(1); + px4_usleep(100000); continue; } } @@ -810,7 +810,7 @@ GPS::run() // Open the UART. If this is successful then the UART is ready to use. if (! _uart->open()) { PX4_ERR("Error opening serial device %s", _port); - px4_sleep(1); + px4_usleep(100000); continue; } diff --git a/src/lib/drivers/device/CMakeLists.txt b/src/lib/drivers/device/CMakeLists.txt index 6aa1707850..7e36f11c06 100644 --- a/src/lib/drivers/device/CMakeLists.txt +++ b/src/lib/drivers/device/CMakeLists.txt @@ -58,6 +58,8 @@ elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type endif() list(APPEND SRCS_PLATFORM posix/SerialImpl.cpp) + list(APPEND SRCS_PLATFORM posix/SerialSBUSImpl.cpp) + list(APPEND SRCS_PLATFORM posix/SerialStandardImpl.cpp) endif() px4_add_library(drivers__device diff --git a/src/lib/drivers/device/Serial.cpp b/src/lib/drivers/device/Serial.cpp index 3d4a3474f3..fd762d60cf 100644 --- a/src/lib/drivers/device/Serial.cpp +++ b/src/lib/drivers/device/Serial.cpp @@ -36,14 +36,9 @@ 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) +Serial::Serial(const char *port, uint32_t baudrate) : + _impl(port, baudrate) { - // If no baudrate was specified then set it to a reasonable default value - if (baudrate == 0) { - (void) _impl.setBaudrate(9600); - } } Serial::~Serial() @@ -90,44 +85,14 @@ bool Serial::setBaudrate(uint32_t baudrate) return _impl.setBaudrate(baudrate); } -ByteSize Serial::getBytesize() const +bool Serial::getSBUSMode() const { - return _impl.getBytesize(); + return _impl.getSBUSMode(); } -bool Serial::setBytesize(ByteSize bytesize) +bool Serial::setSBUSMode(bool enable) { - 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); + return _impl.setSBUSMode(enable); } const char *Serial::getPort() const diff --git a/src/lib/drivers/device/Serial.hpp b/src/lib/drivers/device/Serial.hpp index aa05b0160f..218feaf3ce 100644 --- a/src/lib/drivers/device/Serial.hpp +++ b/src/lib/drivers/device/Serial.hpp @@ -33,6 +33,7 @@ #pragma once +// Bring in the correct platform implementation #ifdef __PX4_NUTTX #include "nuttx/SerialImpl.hpp" #elif defined(__PX4_QURT) @@ -41,25 +42,18 @@ #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); + // Baud rate can be selected with constructor or by using setBaudrate + Serial(const char *port, uint32_t baudrate = 0); virtual ~Serial(); - // Open sets up the port and gets it configured based on desired configuration + // Open sets up the port and gets it configured. Unless an alternate mode + // is selected the port will be configured with parity disabled and 1 stop bit. bool open(); bool isOpen() const; @@ -70,24 +64,16 @@ public: 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; + + // If the port has already been opened it will be reconfigured with a change + // of baudrate. 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); + // SBUS has special configuration considerations and methods so it + // is given a special mode. It has parity enabled and 2 stop bits + bool getSBUSMode() const; + bool setSBUSMode(bool enable); const char *getPort() const; diff --git a/src/lib/drivers/device/nuttx/SerialImpl.cpp b/src/lib/drivers/device/nuttx/SerialImpl.cpp index 42588ed413..b4489271ff 100644 --- a/src/lib/drivers/device/nuttx/SerialImpl.cpp +++ b/src/lib/drivers/device/nuttx/SerialImpl.cpp @@ -39,17 +39,12 @@ #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) +SerialImpl::SerialImpl(const char *port, uint32_t baudrate) { if (port) { strncpy(_port, port, sizeof(_port) - 1); @@ -58,6 +53,13 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P } else { _port[0] = 0; } + + if (baudrate) { + _baudrate = baudrate; + } else { + // If baudrate is zero then choose a reasonable default + _baudrate = 9600; + } } SerialImpl::~SerialImpl() @@ -67,118 +69,122 @@ SerialImpl::~SerialImpl() } } -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; + struct termios uart_config; - if (! validateBaudrate(_baudrate)) { - PX4_ERR("ERR: unknown baudrate: %lu", _baudrate); - return false; - } + if (_SBUSMode) { + if (_baudrate != DEFAULT_SBUS_BAUDRATE) { + PX4_WARN("Warning, SBUS baud rate being set to %lu", _baudrate); + } - switch (_baudrate) { - case 9600: speed = B9600; break; + /* even parity, two stop bits */ + tcgetattr(_serial_fd, &uart_config); + cfsetspeed(&uart_config, _baudrate); + uart_config.c_cflag |= (CSTOPB | PARENB); + tcsetattr(_serial_fd, TCSANOW, &uart_config); - case 19200: speed = B19200; break; + if (board_rc_singlewire(_port)) { + /* only defined in configs capable of IOCTL + * Note It is never turned off + */ +#ifdef TIOCSSINGLEWIRE + ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#endif + } + } else { + /* process baud rate */ + int speed; - case 38400: speed = B38400; break; + switch (_baudrate) { + case 9600: speed = B9600; break; - case 57600: speed = B57600; break; + case 19200: speed = B19200; break; - case 115200: speed = B115200; break; + case 38400: speed = B38400; break; - case 230400: speed = B230400; 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; + case 460800: speed = B460800; break; #ifndef B921600 #define B921600 921600 #endif - case 921600: speed = B921600; break; + case 921600: speed = B921600; break; - default: - PX4_ERR("ERR: unknown baudrate: %lu", _baudrate); - return false; - } + default: + PX4_ERR("ERR: unknown baudrate: %lu", _baudrate); + return false; + } - struct termios uart_config; + int termios_state; - 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; + } - /* 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 ) */ - /* 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); - // - // 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; - // - // 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 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); - /* 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; + } - /* 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 = 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; + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } } return true; @@ -203,6 +209,7 @@ bool SerialImpl::open() // Configure the serial port if (! configure()) { PX4_ERR("failed to configure %s err: %d", _port, errno); + close(); return false; } @@ -218,7 +225,6 @@ bool SerialImpl::isOpen() const bool SerialImpl::close() { - if (_serial_fd >= 0) { ::close(_serial_fd); } @@ -319,64 +325,39 @@ uint32_t SerialImpl::getBaudrate() const 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) { + if (baudrate == _baudrate) { return true; } _baudrate = baudrate; // process baud rate change now if port is already open - if (_open) { - return configure(); + if ((_open) && (configure() != 0)) { + // Configure failed! Close the port + close(); + return false; } return true; } -ByteSize SerialImpl::getBytesize() const +bool SerialImpl::getSBUSMode() const { - return _bytesize; + return _SBUSMode; } -bool SerialImpl::setBytesize(ByteSize bytesize) +bool SerialImpl::setSBUSMode(bool enable) { - return bytesize == ByteSize::EightBits; -} + if (_open) { + PX4_ERR("Cannot configure SBUS mode after port has already been opened"); + return false; + } -Parity SerialImpl::getParity() const -{ - return _parity; -} + _SBUSMode = enable; + _baudrate = DEFAULT_SBUS_BAUDRATE; -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; + return true; } } // namespace device diff --git a/src/lib/drivers/device/nuttx/SerialImpl.hpp b/src/lib/drivers/device/nuttx/SerialImpl.hpp index 799fe54463..1fb649b234 100644 --- a/src/lib/drivers/device/nuttx/SerialImpl.hpp +++ b/src/lib/drivers/device/nuttx/SerialImpl.hpp @@ -34,14 +34,6 @@ #pragma once #include -#include - -#include - -using device::SerialConfig::ByteSize; -using device::SerialConfig::Parity; -using device::SerialConfig::StopBits; -using device::SerialConfig::FlowControl; namespace device { @@ -50,8 +42,7 @@ class SerialImpl { public: - SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, - FlowControl flowcontrol); + SerialImpl(const char *port, uint32_t baudrate); virtual ~SerialImpl(); bool open(); @@ -69,17 +60,8 @@ public: 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); + bool getSBUSMode() const; + bool setSBUSMode(bool enable); private: @@ -91,12 +73,9 @@ private: uint32_t _baudrate{0}; - ByteSize _bytesize{ByteSize::EightBits}; - Parity _parity{Parity::None}; - StopBits _stopbits{StopBits::One}; - FlowControl _flowcontrol{FlowControl::Disabled}; + bool _SBUSMode{false}; + static const uint32_t DEFAULT_SBUS_BAUDRATE{100000}; - bool validateBaudrate(uint32_t baudrate); bool configure(); }; diff --git a/src/lib/drivers/device/posix/SerialImpl.cpp b/src/lib/drivers/device/posix/SerialImpl.cpp index 8d71c5ac40..ff187cb73e 100644 --- a/src/lib/drivers/device/posix/SerialImpl.cpp +++ b/src/lib/drivers/device/posix/SerialImpl.cpp @@ -32,8 +32,10 @@ ****************************************************************************/ #include "SerialImpl.hpp" + +#include + #include // strncpy -#include #include #include #include @@ -43,13 +45,7 @@ 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) +SerialImpl::SerialImpl(const char *port, uint32_t baudrate) { if (port) { strncpy(_port, port, sizeof(_port) - 1); @@ -58,6 +54,13 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P } else { _port[0] = 0; } + + if (baudrate) { + _baudrate = baudrate; + } else { + // If baudrate is zero then choose a reasonable default + _baudrate = 9600; + } } SerialImpl::~SerialImpl() @@ -67,121 +70,13 @@ SerialImpl::~SerialImpl() } } -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; + if (_SBUSMode) { + return _sbus.configure(_serial_fd, _baudrate); } - 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; + return _standard.configure(_serial_fd, _baudrate); } bool SerialImpl::open() @@ -203,6 +98,7 @@ bool SerialImpl::open() // Configure the serial port if (! configure()) { PX4_ERR("failed to configure %s err: %d", _port, errno); + close(); return false; } @@ -218,7 +114,6 @@ bool SerialImpl::isOpen() const bool SerialImpl::close() { - if (_serial_fd >= 0) { ::close(_serial_fd); } @@ -240,7 +135,6 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) if (ret < 0) { PX4_DEBUG("%s read error %d", _port, ret); - } return ret; @@ -321,64 +215,39 @@ uint32_t SerialImpl::getBaudrate() const 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) { + if (baudrate == _baudrate) { return true; } _baudrate = baudrate; // process baud rate change now if port is already open - if (_open) { - return configure(); + if ((_open) && (configure() != 0)) { + // Configure failed! Close the port + close(); + return false; } return true; } -ByteSize SerialImpl::getBytesize() const +bool SerialImpl::getSBUSMode() const { - return _bytesize; + return _SBUSMode; } -bool SerialImpl::setBytesize(ByteSize bytesize) +bool SerialImpl::setSBUSMode(bool enable) { - return bytesize == ByteSize::EightBits; -} + if (_open) { + PX4_ERR("Cannot configure SBUS mode after port has already been opened"); + return false; + } -Parity SerialImpl::getParity() const -{ - return _parity; -} + _SBUSMode = enable; + _baudrate = SerialSBUSImpl::DEFAULT_BAUDRATE; -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; + return true; } } // namespace device diff --git a/src/lib/drivers/device/posix/SerialImpl.hpp b/src/lib/drivers/device/posix/SerialImpl.hpp index cae5aef84e..861a9407a0 100644 --- a/src/lib/drivers/device/posix/SerialImpl.hpp +++ b/src/lib/drivers/device/posix/SerialImpl.hpp @@ -34,14 +34,9 @@ #pragma once #include -#include -#include - -using device::SerialConfig::ByteSize; -using device::SerialConfig::Parity; -using device::SerialConfig::StopBits; -using device::SerialConfig::FlowControl; +#include "SerialSBUSImpl.hpp" +#include "SerialStandardImpl.hpp" namespace device { @@ -50,8 +45,7 @@ class SerialImpl { public: - SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, - FlowControl flowcontrol); + SerialImpl(const char *port, uint32_t baudrate); virtual ~SerialImpl(); bool open(); @@ -69,17 +63,8 @@ public: 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); + bool getSBUSMode() const; + bool setSBUSMode(bool enable); private: @@ -91,12 +76,13 @@ private: uint32_t _baudrate{0}; - ByteSize _bytesize{ByteSize::EightBits}; - Parity _parity{Parity::None}; - StopBits _stopbits{StopBits::One}; - FlowControl _flowcontrol{FlowControl::Disabled}; + bool _SBUSMode{false}; + + // The configuration routines for SBUS versus other needed to be separated + // out because they use different methods with different, conflicting header files + SerialSBUSImpl _sbus; + SerialStandardImpl _standard; - bool validateBaudrate(uint32_t baudrate); bool configure(); }; diff --git a/src/lib/drivers/device/posix/SerialSBUSImpl.cpp b/src/lib/drivers/device/posix/SerialSBUSImpl.cpp new file mode 100644 index 0000000000..47c1df6d2d --- /dev/null +++ b/src/lib/drivers/device/posix/SerialSBUSImpl.cpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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 "SerialSBUSImpl.hpp" + +#include +#include +#include + +namespace device +{ + +bool SerialSBUSImpl::configure(int fd, uint32_t baud) +{ + struct termios2 tio = {}; + + if (ioctl(fd, TCGETS2, &tio)) { + return false; + } + + if (baud != DEFAULT_BAUDRATE) { + PX4_WARN("Warning, SBUS baud rate being set to %u", baud); + } + + /** + * Setting serial port,8E2, non-blocking.100Kbps + */ + tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL + | IXON); + tio.c_iflag |= (INPCK | IGNPAR); + tio.c_oflag &= ~OPOST; + tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD); + /** + * use BOTHER to specify speed directly in c_[io]speed member + */ + tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD); + tio.c_ispeed = baud; + tio.c_ospeed = baud; + tio.c_cc[VMIN] = 25; + tio.c_cc[VTIME] = 0; + + if (ioctl(fd, TCSETS2, &tio)) { + return false; + } + + return true; +} + +} // namespace device diff --git a/src/lib/drivers/device/SerialCommon.hpp b/src/lib/drivers/device/posix/SerialSBUSImpl.hpp similarity index 80% rename from src/lib/drivers/device/SerialCommon.hpp rename to src/lib/drivers/device/posix/SerialSBUSImpl.hpp index bbe9be0ad9..5e85000413 100644 --- a/src/lib/drivers/device/SerialCommon.hpp +++ b/src/lib/drivers/device/posix/SerialSBUSImpl.hpp @@ -33,38 +33,21 @@ #pragma once +#include + namespace device { -namespace SerialConfig + +class SerialSBUSImpl { +public: + SerialSBUSImpl() {}; + virtual ~SerialSBUSImpl() {}; -// ByteSize: number of data bits -enum class ByteSize { - FiveBits = 5, - SixBits = 6, - SevenBits = 7, - EightBits = 8, + bool configure(int fd, uint32_t baud); + + static const uint32_t DEFAULT_BAUDRATE{100000}; }; -// 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/posix/SerialStandardImpl.cpp b/src/lib/drivers/device/posix/SerialStandardImpl.cpp new file mode 100644 index 0000000000..f050772bf4 --- /dev/null +++ b/src/lib/drivers/device/posix/SerialStandardImpl.cpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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 "SerialStandardImpl.hpp" + +#include +#include + +namespace device +{ + +bool SerialStandardImpl::validateBaudrate(uint32_t baudrate) +{ + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); +} + +bool SerialStandardImpl::configure(int fd, uint32_t baud) +{ + /* process baud rate */ + int speed; + + if (! validateBaudrate(baud)) { + PX4_ERR("ERR: unknown baudrate: %u", baud); + return false; + } + + 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 false; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(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(fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +} // namespace device diff --git a/src/lib/drivers/device/posix/SerialStandardImpl.hpp b/src/lib/drivers/device/posix/SerialStandardImpl.hpp new file mode 100644 index 0000000000..283f35477d --- /dev/null +++ b/src/lib/drivers/device/posix/SerialStandardImpl.hpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 + +namespace device +{ + +class SerialStandardImpl +{ +public: + + SerialStandardImpl() {}; + virtual ~SerialStandardImpl() {}; + + bool configure(int fd, uint32_t baud); + +private: + bool validateBaudrate(uint32_t baudrate); + +}; + +} // namespace device diff --git a/src/lib/drivers/device/qurt/SerialImpl.cpp b/src/lib/drivers/device/qurt/SerialImpl.cpp index 5bbe092bcd..9c92a4919f 100644 --- a/src/lib/drivers/device/qurt/SerialImpl.cpp +++ b/src/lib/drivers/device/qurt/SerialImpl.cpp @@ -40,13 +40,7 @@ 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) +SerialImpl::SerialImpl(const char *port, uint32_t baudrate) { if (port) { strncpy(_port, port, sizeof(_port) - 1); @@ -55,6 +49,14 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P } else { _port[0] = 0; } + + if (baudrate) { + _baudrate = baudrate; + } else { + // If baudrate is zero then choose a reasonable default. + // The default is the GPS UBX M10 default rate. + _baudrate = 115200; + } } SerialImpl::~SerialImpl() @@ -64,26 +66,6 @@ SerialImpl::~SerialImpl() } } -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. @@ -92,36 +74,12 @@ bool SerialImpl::open() _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); + PX4_ERR("failed to open %s at baudrate %u, fd: %d", _port, _baudrate, serial_fd); + close(); return false; } else { @@ -263,11 +221,6 @@ uint32_t SerialImpl::getBaudrate() const 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; @@ -283,44 +236,4 @@ bool SerialImpl::setBaudrate(uint32_t baudrate) 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 index 1bdea98a48..f4463fdd7a 100644 --- a/src/lib/drivers/device/qurt/SerialImpl.hpp +++ b/src/lib/drivers/device/qurt/SerialImpl.hpp @@ -33,14 +33,7 @@ #pragma once -#include - -#include - -using device::SerialConfig::ByteSize; -using device::SerialConfig::Parity; -using device::SerialConfig::StopBits; -using device::SerialConfig::FlowControl; +#include namespace device { @@ -49,8 +42,7 @@ class SerialImpl { public: - SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, - FlowControl flowcontrol); + SerialImpl(const char *port, uint32_t baudrate); virtual ~SerialImpl(); bool open(); @@ -64,22 +56,13 @@ public: 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); + // Cannot configure Qurt UARTs for SBUS! + bool getSBUSMode() const { return false; } + bool setSBUSMode(bool enable) { return false; } private: @@ -91,18 +74,7 @@ private: 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