From 96c05ac23acc5c74807e6f206c649bc2121574c1 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 15 Jun 2023 11:52:04 -0700 Subject: [PATCH 01/12] First working version. Tested on ModalAI FlightCore V2 --- platforms/common/CMakeLists.txt | 3 +- platforms/common/Serial.cpp | 114 ++++++ .../include/px4_platform_common/Serial.hpp | 102 +++++ .../px4_platform_common/SerialCommon.hpp | 38 ++ platforms/nuttx/src/px4/common/SerialImpl.cpp | 367 ++++++++++++++++++ .../src/px4/common/include/SerialImpl.hpp | 81 ++++ .../nuttx/src/px4/common/px4_layer.cmake | 1 + platforms/posix/include/SerialImpl.hpp | 81 ++++ platforms/posix/src/px4/common/CMakeLists.txt | 1 + platforms/posix/src/px4/common/SerialImpl.cpp | 365 +++++++++++++++++ platforms/qurt/include/SerialImpl.hpp | 78 ++++ platforms/qurt/src/px4/CMakeLists.txt | 1 + platforms/qurt/src/px4/SerialImpl.cpp | 294 ++++++++++++++ src/drivers/gps/gps.cpp | 281 ++++++-------- 14 files changed, 1640 insertions(+), 167 deletions(-) create mode 100644 platforms/common/Serial.cpp create mode 100644 platforms/common/include/px4_platform_common/Serial.hpp create mode 100644 platforms/common/include/px4_platform_common/SerialCommon.hpp create mode 100644 platforms/nuttx/src/px4/common/SerialImpl.cpp create mode 100644 platforms/nuttx/src/px4/common/include/SerialImpl.hpp create mode 100644 platforms/posix/include/SerialImpl.hpp create mode 100644 platforms/posix/src/px4/common/SerialImpl.cpp create mode 100644 platforms/qurt/include/SerialImpl.hpp create mode 100644 platforms/qurt/src/px4/SerialImpl.cpp diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index a6e06b9d47..5bdd0d580b 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -52,9 +52,10 @@ add_library(px4_platform STATIC px4_cli.cpp shutdown.cpp spi.cpp + Serial.cpp ${SRCS} ) -target_link_libraries(px4_platform prebuild_targets px4_work_queue) +target_link_libraries(px4_platform prebuild_targets px4_work_queue px4_layer) if(NOT "${PX4_BOARD}" MATCHES "io-v2") add_subdirectory(uORB) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp new file mode 100644 index 0000000000..d565e31177 --- /dev/null +++ b/platforms/common/Serial.cpp @@ -0,0 +1,114 @@ +#include + +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(); +} + +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); +} + +} // namespace device diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp new file mode 100644 index 0000000000..f4b863facf --- /dev/null +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 + +#include + +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); + + // Perhaps this can be removed? To change the port you need to create a new object. + const char *getPort() const; + bool setPort(const char *port); + + // 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); + + // 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/platforms/common/include/px4_platform_common/SerialCommon.hpp b/platforms/common/include/px4_platform_common/SerialCommon.hpp new file mode 100644 index 0000000000..9c36a21b14 --- /dev/null +++ b/platforms/common/include/px4_platform_common/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/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp new file mode 100644 index 0000000000..421aa5d1ac --- /dev/null +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -0,0 +1,367 @@ + +#include +#include // strncpy +#include // strncpy +#include +#include +#include +#include +#include + +#define MODULE_NAME "SerialImpl" + +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::configure() { + /* process baud rate */ + int speed; + + 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 */ + 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) { + 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; + } + + // Configure the serial port if a baudrate has been configured + if (_baudrate) { + if ( ! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + } + + _serial_fd = serial_fd; + _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); + + } else { + // Track total bytes read + _bytes_read += 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); + + } else { + // Track total bytes written + _bytes_written += written; + } + + return written; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +bool SerialImpl::setPort(const char *port) +{ + if (strcmp(port, _port) == 0) { + return true; + } + + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + // If old port is already opened then close it and reopen it on new port + if (_open) { + close(); + return open(); + } + + return true; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + // 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) +{ + if (bytesize != ByteSize::EightBits) { + return false; + } + + return true; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + if (parity != Parity::None) { + return false; + } + + return true; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + if (stopbits != StopBits::One) { + return false; + } + + return true; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + if (flowcontrol != FlowControl::Disabled) { + return false; + } + + return true; +} + +} // namespace device diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp new file mode 100644 index 0000000000..ba085f2ef8 --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -0,0 +1,81 @@ + + +#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; + 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}; + + size_t _bytes_read{0}; + size_t _bytes_written{0}; + + 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 configure(); + + // 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 diff --git a/platforms/nuttx/src/px4/common/px4_layer.cmake b/platforms/nuttx/src/px4/common/px4_layer.cmake index 69fd1a007d..d1747d4935 100644 --- a/platforms/nuttx/src/px4/common/px4_layer.cmake +++ b/platforms/nuttx/src/px4/common/px4_layer.cmake @@ -3,6 +3,7 @@ add_library(px4_layer ${KERNEL_SRCS} cdc_acm_check.cpp + SerialImpl.cpp ) target_link_libraries(px4_layer diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp new file mode 100644 index 0000000000..ba085f2ef8 --- /dev/null +++ b/platforms/posix/include/SerialImpl.hpp @@ -0,0 +1,81 @@ + + +#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; + 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}; + + size_t _bytes_read{0}; + size_t _bytes_written{0}; + + 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 configure(); + + // 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 diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 34b65cdf44..b96705eae3 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -46,6 +46,7 @@ add_library(px4_layer drv_hrt.cpp cpuload.cpp print_load.cpp + SerialImpl.cpp ) target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp new file mode 100644 index 0000000000..59b6c9fad6 --- /dev/null +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -0,0 +1,365 @@ + +#include +#include // strncpy +#include // strncpy +#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::configure() { + /* process baud rate */ + int speed; + + 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 */ + 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) { + 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; + } + + // Configure the serial port if a baudrate has been configured + if (_baudrate) { + if ( ! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + } + + _serial_fd = serial_fd; + _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); + + } else { + // Track total bytes read + _bytes_read += 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); + + } else { + // Track total bytes written + _bytes_written += written; + } + + return written; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +bool SerialImpl::setPort(const char *port) +{ + if (strcmp(port, _port) == 0) { + return true; + } + + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + // If old port is already opened then close it and reopen it on new port + if (_open) { + close(); + return open(); + } + + return true; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + // 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) +{ + if (bytesize != ByteSize::EightBits) { + return false; + } + + return true; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + if (parity != Parity::None) { + return false; + } + + return true; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + if (stopbits != StopBits::One) { + return false; + } + + return true; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + if (flowcontrol != FlowControl::Disabled) { + return false; + } + + return true; +} + +} // namespace device diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp new file mode 100644 index 0000000000..ebede1f5ed --- /dev/null +++ b/platforms/qurt/include/SerialImpl.hpp @@ -0,0 +1,78 @@ + + +#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}; + + size_t _bytes_read{0}; + size_t _bytes_written{0}; + + 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}; + + // 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 diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index e685b8d42a..8a25322755 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS px4_qurt_impl.cpp main.cpp qurt_log.cpp + SerialImpl.cpp ) add_library(px4_layer diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp new file mode 100644 index 0000000000..cf8f4af0ae --- /dev/null +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -0,0 +1,294 @@ + +#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; + } + + // Start off with a valid bitrate to make sure open can succeed + if (_baudrate == 0) _baudrate = 9600; + +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +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 (_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); + + } 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) +{ + 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); + + } else { + _bytes_written += ret_write; + } + + return ret_write; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +bool SerialImpl::setPort(const char *port) +{ + if (strcmp(port, _port) == 0) { + return true; + } + + return false; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + // 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) +{ + if (bytesize != ByteSize::EightBits) { + return false; + } + + return true; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + if (parity != Parity::None) { + return false; + } + + return true; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + if (stopbits != StopBits::One) { + return false; + } + + return true; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + if (flowcontrol != FlowControl::Disabled) { + return false; + } + + return true; +} + +} // namespace device diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4ed29a8c6e..95ef5d06bb 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -45,7 +45,6 @@ #include #endif -#include #include #include @@ -57,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -81,6 +81,7 @@ #include #endif /* __PX4_LINUX */ +using namespace device; using namespace time_literals; #define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error @@ -169,7 +170,8 @@ public: void reset_if_scheduled(); private: - int _serial_fd{-1}; ///< serial interface to GPS + int _spi_fd{-1}; ///< SPI interface to GPS + Serial *_uart = nullptr; ///< UART 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 @@ -403,10 +405,19 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) return num_read; } - case GPSCallbackType::writeDeviceData: - gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); + 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); + int ret = 0; + if (gps->_uart) { + ret = gps->_uart->write( (void*) data1, (size_t) data2); + + } else if (gps->_spi_fd >= 0) { + ret = ::write(gps->_spi_fd, data1, (size_t)data2); + } + + return ret; + } case GPSCallbackType::setBaudrate: return gps->setBaudrate(data2); @@ -449,72 +460,74 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) { + int ret = 0; + const unsigned character_count = 32; // minimum bytes that we want to read + const int max_timeout = 50; + int _timeout = math::min(max_timeout, timeout); + handleInjectDataTopic(); + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + ret = _uart->readAtLeast(buf, buf_length, character_count, _timeout); + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { + +// Qurt does not support poll for serial devices (nor external SPI devices for that matter) #if !defined(__PX4_QURT) + /* For non QURT, use the usual polling. */ - /* For non QURT, use the usual polling. */ + //Poll only for the SPI 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 - //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 = _spi_fd; + fds[0].events = POLLIN; - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; + ret = poll(fds, sizeof(fds) / sizeof(fds[0]), _timeout); - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout)); - - 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); + 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. + */ + 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); - } + int err = 0; + int bytes_available = 0; + err = ::ioctl(_spi_fd, FIONREAD, (unsigned long)&bytes_available); + if (err != 0 || bytes_available < (int)character_count) { + px4_usleep(sleeptime); + } #else - px4_usleep(sleeptime); + px4_usleep(sleeptime); #endif - ret = ::read(_serial_fd, buf, buf_length); + ret = ::read(_spi_fd, buf, buf_length); - if (ret > 0) { - _num_bytes_read += ret; + if (ret > 0) { + _num_bytes_read += ret; + } + + } else { + ret = -1; } - - } else { - ret = -1; } +#endif } 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() @@ -583,105 +596,31 @@ 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); + size_t written = 0; + + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + written = _uart->write((const void*) data, len); + + } else if (_interface == GPSHelper::Interface::SPI) { + written = ::write(_spi_fd, data, len); + ::fsync(_spi_fd); + } + 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 (_interface == GPSHelper::Interface::UART) { + if (_uart) { + return _uart->setBaudrate(baud); + } + } else if (_interface == GPSHelper::Interface::SPI) { + // Can't set the baudrate on a SPI port but just return a success + 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() @@ -840,33 +779,39 @@ GPS::run() _helper = nullptr; } - if (_serial_fd < 0) { - /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) { + _uart = new Serial(_port, _configured_baudrate); - if (_serial_fd < 0) { - PX4_ERR("failed to open %s err: %d", _port, errno); + if (_uart == nullptr) { + PX4_ERR("Error creating serial device %s", _port); + px4_sleep(1); + continue; + } + + _uart->open(); + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)){ + _spi_fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (_spi_fd < 0) { + PX4_ERR("failed to open SPI port %s err: %d", _port, errno); px4_sleep(1); continue; } #ifdef __PX4_LINUX + int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) + int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - 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); - } + if (status_value < 0) { + PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); } + status_value = ::ioctl(_spi_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 */ } @@ -1056,10 +1001,14 @@ GPS::run() } } - if (_serial_fd >= 0) { - ::close(_serial_fd); - _serial_fd = -1; - } + if ((_interface == GPSHelper::Interface::UART) && (_uart)){ + _uart->close(); + delete _uart; + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)){ + ::close(_spi_fd); + _spi_fd = -1; + } if (_mode_auto) { switch (_mode) { From 4c5e05de137315bbb80dffa87acac38b843a240d Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Fri, 16 Jun 2023 13:44:01 -0700 Subject: [PATCH 02/12] Fixed formatting and made SPI a Linux only choice to GPS --- platforms/nuttx/src/px4/common/SerialImpl.cpp | 10 ++- platforms/posix/src/px4/common/SerialImpl.cpp | 10 ++- platforms/qurt/src/px4/SerialImpl.cpp | 13 ++-- src/drivers/gps/gps.cpp | 67 +++++++++++-------- 4 files changed, 61 insertions(+), 39 deletions(-) diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 421aa5d1ac..83c5f354ff 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -24,6 +24,7 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P if (port) { strncpy(_port, port, sizeof(_port) - 1); _port[sizeof(_port) - 1] = '\0'; + } else { _port[0] = 0; } @@ -37,7 +38,8 @@ SerialImpl::~SerialImpl() } } -bool SerialImpl::configure() { +bool SerialImpl::configure() +{ /* process baud rate */ int speed; @@ -72,6 +74,7 @@ bool SerialImpl::configure() { } struct termios uart_config; + int termios_state; /* fill the struct for the new configuration */ @@ -148,7 +151,7 @@ bool SerialImpl::open() // Configure the serial port if a baudrate has been configured if (_baudrate) { - if ( ! configure()) { + if (! configure()) { PX4_ERR("failed to configure %s err: %d", _port, errno); return false; } @@ -220,7 +223,8 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char fds[0].events = POLLIN; hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); - if (remaining_time <= 0) break; + + if (remaining_time <= 0) { break; } int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 59b6c9fad6..71aa15b8bc 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -22,6 +22,7 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P if (port) { strncpy(_port, port, sizeof(_port) - 1); _port[sizeof(_port) - 1] = '\0'; + } else { _port[0] = 0; } @@ -35,7 +36,8 @@ SerialImpl::~SerialImpl() } } -bool SerialImpl::configure() { +bool SerialImpl::configure() +{ /* process baud rate */ int speed; @@ -70,6 +72,7 @@ bool SerialImpl::configure() { } struct termios uart_config; + int termios_state; /* fill the struct for the new configuration */ @@ -146,7 +149,7 @@ bool SerialImpl::open() // Configure the serial port if a baudrate has been configured if (_baudrate) { - if ( ! configure()) { + if (! configure()) { PX4_ERR("failed to configure %s err: %d", _port, errno); return false; } @@ -218,7 +221,8 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char fds[0].events = POLLIN; hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); - if (remaining_time <= 0) break; + + if (remaining_time <= 0) { break; } int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index cf8f4af0ae..cfdbddc4ba 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -19,12 +19,13 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P if (port) { strncpy(_port, port, sizeof(_port) - 1); _port[sizeof(_port) - 1] = '\0'; + } else { _port[0] = 0; } // Start off with a valid bitrate to make sure open can succeed - if (_baudrate == 0) _baudrate = 9600; + if (_baudrate == 0) { _baudrate = 9600; } } @@ -69,6 +70,7 @@ bool SerialImpl::open() 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); } @@ -104,7 +106,7 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) return -1; } - int ret_read = qurt_uart_read(_serial_fd, (char*) buffer, buffer_size, 500); + 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); @@ -144,7 +146,7 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char return total_bytes_read; } } - + int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); if (current_bytes_read < 0) { @@ -169,8 +171,9 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char 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_until_timeout < (int64_t) time_to_sleep)) { time_to_sleep = time_until_timeout; } @@ -187,7 +190,7 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) return -1; } - int ret_write = qurt_uart_write(_serial_fd, (const char*) buffer, buffer_size); + 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); diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 95ef5d06bb..09eb53fdd2 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -170,7 +170,9 @@ public: void reset_if_scheduled(); private: - int _spi_fd{-1}; ///< SPI interface to GPS +#ifdef __PX4_LINUX + int _spi_fd {-1}; ///< SPI interface to GPS +#endif Serial *_uart = nullptr; ///< UART interface to GPS unsigned _baudrate{0}; ///< current baudrate const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect) @@ -331,8 +333,11 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac 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 +#ifdef __PX4_LINUX + } else if (_interface == GPSHelper::Interface::SPI) { set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI); +#endif } if (_mode == gps_driver_mode_t::None) { @@ -409,11 +414,15 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); int ret = 0; + if (gps->_uart) { - ret = gps->_uart->write( (void*) data1, (size_t) data2); + ret = gps->_uart->write((void *) data1, (size_t) data2); + +#ifdef __PX4_LINUX } else if (gps->_spi_fd >= 0) { ret = ::write(gps->_spi_fd, data1, (size_t)data2); +#endif } return ret; @@ -470,14 +479,13 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) if ((_interface == GPSHelper::Interface::UART) && (_uart)) { ret = _uart->readAtLeast(buf, buf_length, character_count, _timeout); +// SPI is only supported on LInux +#if defined(__PX4_LINUX) + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { -// Qurt does not support poll for serial devices (nor external SPI devices for that matter) -#if !defined(__PX4_QURT) - /* For non QURT, use the usual polling. */ - //Poll only for the SPI 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 + //so ideally we would poll on both, the SPI 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. @@ -502,17 +510,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) 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(_spi_fd, FIONREAD, (unsigned long)&bytes_available); - - if (err != 0 || bytes_available < (int)character_count) { - px4_usleep(sleeptime); - } -#else px4_usleep(sleeptime); -#endif ret = ::read(_spi_fd, buf, buf_length); @@ -524,6 +522,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) ret = -1; } } + #endif } @@ -599,11 +598,14 @@ bool GPS::injectData(uint8_t *data, size_t len) size_t written = 0; if ((_interface == GPSHelper::Interface::UART) && (_uart)) { - written = _uart->write((const void*) data, len); + written = _uart->write((const void *) data, len); + +#ifdef __PX4_LINUX } else if (_interface == GPSHelper::Interface::SPI) { written = ::write(_spi_fd, data, len); ::fsync(_spi_fd); +#endif } return written == len; @@ -615,9 +617,13 @@ int GPS::setBaudrate(unsigned baud) if (_uart) { return _uart->setBaudrate(baud); } + +#ifdef __PX4_LINUX + } else if (_interface == GPSHelper::Interface::SPI) { // Can't set the baudrate on a SPI port but just return a success return 0; +#endif } return -1; @@ -790,7 +796,9 @@ GPS::run() _uart->open(); - } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)){ +#ifdef __PX4_LINUX + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) { _spi_fd = ::open(_port, O_RDWR | O_NOCTTY); if (_spi_fd < 0) { @@ -799,7 +807,6 @@ GPS::run() continue; } -#ifdef __PX4_LINUX int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); @@ -812,6 +819,7 @@ GPS::run() if (status_value < 0) { PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); } + #endif /* __PX4_LINUX */ } @@ -1001,14 +1009,17 @@ GPS::run() } } - if ((_interface == GPSHelper::Interface::UART) && (_uart)){ + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { _uart->close(); delete _uart; - } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)){ +#ifdef __PX4_LINUX + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { ::close(_spi_fd); _spi_fd = -1; - } +#endif + } if (_mode_auto) { switch (_mode) { @@ -1426,12 +1437,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) break; case 'i': - if (!strcmp(myoptarg, "spi")) { - interface = GPSHelper::Interface::SPI; - - } else if (!strcmp(myoptarg, "uart")) { + if (!strcmp(myoptarg, "uart")) { interface = GPSHelper::Interface::UART; - +#ifdef __PX4_LINUX + } else if (!strcmp(myoptarg, "spi")) { + interface = GPSHelper::Interface::SPI; +#endif } else { PX4_ERR("unknown interface: %s", myoptarg); error_flag = true; From 960c2ab797436374fa52854c08ae6cdd428610e1 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 6 Jul 2023 11:15:26 -0700 Subject: [PATCH 03/12] Fixed another GPS SPI option to make it only for Linux builds --- src/drivers/gps/gps.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 09eb53fdd2..19ab352ca2 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1450,12 +1450,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) break; case 'j': - if (!strcmp(myoptarg, "spi")) { - interface_secondary = GPSHelper::Interface::SPI; - - } else if (!strcmp(myoptarg, "uart")) { + if (!strcmp(myoptarg, "uart")) { interface_secondary = GPSHelper::Interface::UART; - +#ifdef __PX4_LINUX + } else if (!strcmp(myoptarg, "spi")) { + interface_secondary = GPSHelper::Interface::SPI; +#endif } else { PX4_ERR("unknown interface for secondary: %s", myoptarg); error_flag = true; From 8b43a8a5f640b6c77c8d75b738e3d19a75295e79 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 6 Jul 2023 11:16:35 -0700 Subject: [PATCH 04/12] Fixed linking error in SLPI build --- platforms/qurt/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt index d85f3fcb7b..0e9757a5d1 100644 --- a/platforms/qurt/CMakeLists.txt +++ b/platforms/qurt/CMakeLists.txt @@ -50,5 +50,4 @@ add_library(px4 SHARED target_link_libraries(px4 modules__muorb__slpi ${module_libraries} - px4_layer ) From 83f3bc4dffc58f2834335bfc99caf0323ae97641 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Fri, 7 Jul 2023 11:01:44 -0700 Subject: [PATCH 05/12] Fixed Clang tidy errors --- platforms/nuttx/src/px4/common/SerialImpl.cpp | 24 ++++--------------- platforms/posix/src/px4/common/SerialImpl.cpp | 24 ++++--------------- platforms/qurt/src/px4/SerialImpl.cpp | 24 ++++--------------- 3 files changed, 12 insertions(+), 60 deletions(-) diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 83c5f354ff..29134dc80a 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -319,11 +319,7 @@ ByteSize SerialImpl::getBytesize() const bool SerialImpl::setBytesize(ByteSize bytesize) { - if (bytesize != ByteSize::EightBits) { - return false; - } - - return true; + return bytesize == ByteSize::EightBits; } Parity SerialImpl::getParity() const @@ -333,11 +329,7 @@ Parity SerialImpl::getParity() const bool SerialImpl::setParity(Parity parity) { - if (parity != Parity::None) { - return false; - } - - return true; + return parity == Parity::None; } StopBits SerialImpl::getStopbits() const @@ -347,11 +339,7 @@ StopBits SerialImpl::getStopbits() const bool SerialImpl::setStopbits(StopBits stopbits) { - if (stopbits != StopBits::One) { - return false; - } - - return true; + return stopbits == StopBits::One; } FlowControl SerialImpl::getFlowcontrol() const @@ -361,11 +349,7 @@ FlowControl SerialImpl::getFlowcontrol() const bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) { - if (flowcontrol != FlowControl::Disabled) { - return false; - } - - return true; + return flowcontrol == FlowControl::Disabled; } } // namespace device diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 71aa15b8bc..7e181de881 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -317,11 +317,7 @@ ByteSize SerialImpl::getBytesize() const bool SerialImpl::setBytesize(ByteSize bytesize) { - if (bytesize != ByteSize::EightBits) { - return false; - } - - return true; + return bytesize == ByteSize::EightBits; } Parity SerialImpl::getParity() const @@ -331,11 +327,7 @@ Parity SerialImpl::getParity() const bool SerialImpl::setParity(Parity parity) { - if (parity != Parity::None) { - return false; - } - - return true; + return parity == Parity::None; } StopBits SerialImpl::getStopbits() const @@ -345,11 +337,7 @@ StopBits SerialImpl::getStopbits() const bool SerialImpl::setStopbits(StopBits stopbits) { - if (stopbits != StopBits::One) { - return false; - } - - return true; + return stopbits == StopBits::One; } FlowControl SerialImpl::getFlowcontrol() const @@ -359,11 +347,7 @@ FlowControl SerialImpl::getFlowcontrol() const bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) { - if (flowcontrol != FlowControl::Disabled) { - return false; - } - - return true; + return flowcontrol == FlowControl::Disabled; } } // namespace device diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index cfdbddc4ba..a231d241ac 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -245,11 +245,7 @@ ByteSize SerialImpl::getBytesize() const bool SerialImpl::setBytesize(ByteSize bytesize) { - if (bytesize != ByteSize::EightBits) { - return false; - } - - return true; + return bytesize == ByteSize::EightBits; } Parity SerialImpl::getParity() const @@ -259,11 +255,7 @@ Parity SerialImpl::getParity() const bool SerialImpl::setParity(Parity parity) { - if (parity != Parity::None) { - return false; - } - - return true; + return parity == Parity::None; } StopBits SerialImpl::getStopbits() const @@ -273,11 +265,7 @@ StopBits SerialImpl::getStopbits() const bool SerialImpl::setStopbits(StopBits stopbits) { - if (stopbits != StopBits::One) { - return false; - } - - return true; + return stopbits == StopBits::One; } FlowControl SerialImpl::getFlowcontrol() const @@ -287,11 +275,7 @@ FlowControl SerialImpl::getFlowcontrol() const bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) { - if (flowcontrol != FlowControl::Disabled) { - return false; - } - - return true; + return flowcontrol == FlowControl::Disabled; } } // namespace device From 14a3df8b60ad15c09a66232a8292bc2fefef7cf3 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Wed, 19 Jul 2023 17:21:59 -0700 Subject: [PATCH 06/12] Changed the default baudrate handling around a little to make it cleaner --- platforms/common/Serial.cpp | 12 ++++------ platforms/qurt/src/px4/SerialImpl.cpp | 4 ---- src/drivers/gps/gps.cpp | 33 ++++++++++++++++++++------- 3 files changed, 29 insertions(+), 20 deletions(-) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp index d565e31177..bee87dbf51 100644 --- a/platforms/common/Serial.cpp +++ b/platforms/common/Serial.cpp @@ -7,14 +7,10 @@ Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity pa 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 + // If no baudrate was specified then set it to a reasonable default value + if (baudrate == 0) { + (void) _impl.setBaudrate(9600); + } } Serial::~Serial() diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index a231d241ac..24eb5f4e8b 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -23,10 +23,6 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P } else { _port[0] = 0; } - - // Start off with a valid bitrate to make sure open can succeed - if (_baudrate == 0) { _baudrate = 9600; } - } SerialImpl::~SerialImpl() diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 19ab352ca2..645b1d7737 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -472,12 +472,12 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) int ret = 0; const unsigned character_count = 32; // minimum bytes that we want to read const int max_timeout = 50; - int _timeout = math::min(max_timeout, timeout); + int timeout_adjusted = math::min(max_timeout, timeout); handleInjectDataTopic(); if ((_interface == GPSHelper::Interface::UART) && (_uart)) { - ret = _uart->readAtLeast(buf, buf_length, character_count, _timeout); + ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted); // SPI is only supported on LInux #if defined(__PX4_LINUX) @@ -495,7 +495,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) fds[0].fd = _spi_fd; fds[0].events = POLLIN; - ret = poll(fds, sizeof(fds) / sizeof(fds[0]), _timeout); + ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted); if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -614,8 +614,8 @@ bool GPS::injectData(uint8_t *data, size_t len) int GPS::setBaudrate(unsigned baud) { if (_interface == GPSHelper::Interface::UART) { - if (_uart) { - return _uart->setBaudrate(baud); + if ((_uart) && (_uart->setBaudrate(baud))) { + return 0; } #ifdef __PX4_LINUX @@ -786,7 +786,9 @@ GPS::run() } if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) { - _uart = new Serial(_port, _configured_baudrate); + + // Create the UART port instance + _uart = new Serial(_port); if (_uart == nullptr) { PX4_ERR("Error creating serial device %s", _port); @@ -794,7 +796,22 @@ GPS::run() continue; } - _uart->open(); + // Configure the desired baudrate if one was specified by the user. + // Otherwise the default baudrate will be used. + if (_configured_baudrate) { + if (! _uart->setBaudrate(_configured_baudrate)) { + PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port); + px4_sleep(1); + continue; + } + } + + // 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); + continue; + } #ifdef __PX4_LINUX @@ -1010,7 +1027,7 @@ GPS::run() } if ((_interface == GPSHelper::Interface::UART) && (_uart)) { - _uart->close(); + (void) _uart->close(); delete _uart; #ifdef __PX4_LINUX From 1b92db9d896af7eec4dbe1a1f7d5513df9a40ba0 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 20 Jul 2023 10:26:58 -0700 Subject: [PATCH 07/12] Adding some copyright notices and general cleanup --- platforms/common/Serial.cpp | 43 ++++++++++++++----- .../include/px4_platform_common/Serial.hpp | 9 +--- .../px4_platform_common/SerialCommon.hpp | 34 ++++++++++++++- platforms/nuttx/src/px4/common/SerialImpl.cpp | 34 ++++++++++++++- .../src/px4/common/include/SerialImpl.hpp | 38 +++++++++++++--- 5 files changed, 132 insertions(+), 26 deletions(-) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp index bee87dbf51..79e1625169 100644 --- a/platforms/common/Serial.cpp +++ b/platforms/common/Serial.cpp @@ -1,3 +1,36 @@ +/**************************************************************************** + * + * 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 namespace device @@ -47,16 +80,6 @@ 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(); diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp index f4b863facf..aa55e9a321 100644 --- a/platforms/common/include/px4_platform_common/Serial.hpp +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * 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 @@ -64,10 +64,6 @@ public: ssize_t write(const void *buffer, size_t buffer_size); - // Perhaps this can be removed? To change the port you need to create a new object. - const char *getPort() const; - bool setPort(const char *port); - // 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. @@ -87,9 +83,6 @@ public: FlowControl getFlowcontrol() const; bool setFlowcontrol(FlowControl flowcontrol); - // printStatus() - // port, read bytes, write bytes - private: // Disable copy constructors Serial(const Serial &); diff --git a/platforms/common/include/px4_platform_common/SerialCommon.hpp b/platforms/common/include/px4_platform_common/SerialCommon.hpp index 9c36a21b14..bbe9be0ad9 100644 --- a/platforms/common/include/px4_platform_common/SerialCommon.hpp +++ b/platforms/common/include/px4_platform_common/SerialCommon.hpp @@ -1,3 +1,35 @@ +/**************************************************************************** + * + * 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 @@ -34,5 +66,5 @@ enum class FlowControl { Enabled = 1, }; -} // namespace Serial +} // namespace SerialConfig } // namespace device diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 29134dc80a..4794e66602 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -1,7 +1,39 @@ +/**************************************************************************** + * + * 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 // strncpy +#include #include #include #include diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index ba085f2ef8..1cf9fd3bcd 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -1,4 +1,35 @@ - +/**************************************************************************** + * + * 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 @@ -71,11 +102,6 @@ private: bool configure(); - // 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 From 4672106699dfa94cd4733a55f947b3525f6647e0 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 20 Jul 2023 13:11:35 -0700 Subject: [PATCH 08/12] Added baudrate validation functions to the Serial implementations --- platforms/nuttx/src/px4/common/SerialImpl.cpp | 26 ++++++++++++++++ .../src/px4/common/include/SerialImpl.hpp | 1 + platforms/posix/include/SerialImpl.hpp | 1 + platforms/posix/src/px4/common/SerialImpl.cpp | 26 ++++++++++++++++ platforms/qurt/include/SerialImpl.hpp | 2 ++ platforms/qurt/src/px4/SerialImpl.cpp | 30 +++++++++++++++++++ 6 files changed, 86 insertions(+) diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 4794e66602..4fcf2e499c 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -70,11 +70,32 @@ SerialImpl::~SerialImpl() } } +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + if ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)) { + return true; + } + + return false; +} + 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; @@ -329,6 +350,11 @@ 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) { return true; diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index 1cf9fd3bcd..4476cb20a9 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -100,6 +100,7 @@ private: StopBits _stopbits{StopBits::One}; FlowControl _flowcontrol{FlowControl::Disabled}; + bool validateBaudrate(uint32_t baudrate); bool configure(); }; diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp index ba085f2ef8..93765efac7 100644 --- a/platforms/posix/include/SerialImpl.hpp +++ b/platforms/posix/include/SerialImpl.hpp @@ -69,6 +69,7 @@ private: StopBits _stopbits{StopBits::One}; FlowControl _flowcontrol{FlowControl::Disabled}; + bool validateBaudrate(uint32_t baudrate); bool configure(); // Mutex used to lock the read functions diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 7e181de881..ab48a1a58a 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -36,11 +36,32 @@ SerialImpl::~SerialImpl() } } +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + if ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)) { + return true; + } + + return false; +} + 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; @@ -295,6 +316,11 @@ 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) { return true; diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp index ebede1f5ed..dadda42e77 100644 --- a/platforms/qurt/include/SerialImpl.hpp +++ b/platforms/qurt/include/SerialImpl.hpp @@ -68,6 +68,8 @@ private: 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; diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index 24eb5f4e8b..9ce43e94f2 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -32,6 +32,26 @@ 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. @@ -40,6 +60,11 @@ 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; @@ -219,6 +244,11 @@ 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; From 884763b57775a7c1a96546f03135a3d1da3cf2e6 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 20 Jul 2023 13:14:28 -0700 Subject: [PATCH 09/12] Fixed formatting --- platforms/nuttx/src/px4/common/SerialImpl.cpp | 14 ++++++------ platforms/posix/src/px4/common/SerialImpl.cpp | 14 ++++++------ platforms/qurt/src/px4/SerialImpl.cpp | 22 +++++++++---------- 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 4fcf2e499c..e8df9fcead 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -73,13 +73,13 @@ SerialImpl::~SerialImpl() bool SerialImpl::validateBaudrate(uint32_t baudrate) { if ((baudrate == 9600) || - (baudrate == 19200) || - (baudrate == 38400) || - (baudrate == 57600) || - (baudrate == 115200) || - (baudrate == 230400) || - (baudrate == 460800) || - (baudrate == 921600)) { + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)) { return true; } diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index ab48a1a58a..ef76c2ebf7 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -39,13 +39,13 @@ SerialImpl::~SerialImpl() bool SerialImpl::validateBaudrate(uint32_t baudrate) { if ((baudrate == 9600) || - (baudrate == 19200) || - (baudrate == 38400) || - (baudrate == 57600) || - (baudrate == 115200) || - (baudrate == 230400) || - (baudrate == 460800) || - (baudrate == 921600)) { + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)) { return true; } diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index 9ce43e94f2..f4606a8046 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -35,17 +35,17 @@ 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)) { + (baudrate != 38400) && + (baudrate != 57600) && + (baudrate != 115200) && + (baudrate != 230400) && + (baudrate != 250000) && + (baudrate != 420000) && + (baudrate != 460800) && + (baudrate != 921600) && + (baudrate != 1000000) && + (baudrate != 1843200) && + (baudrate != 2000000)) { return false; } From 9c5b1477d48be2ed0779dba85d04cac85293fc63 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 20 Jul 2023 13:21:27 -0700 Subject: [PATCH 10/12] Added getPort back in to Serial interface. Only setPort needed to be removed --- platforms/common/Serial.cpp | 5 +++++ .../include/px4_platform_common/Serial.hpp | 2 ++ platforms/nuttx/src/px4/common/SerialImpl.cpp | 18 ------------------ .../src/px4/common/include/SerialImpl.hpp | 1 - 4 files changed, 7 insertions(+), 19 deletions(-) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp index 79e1625169..2f93a66a6e 100644 --- a/platforms/common/Serial.cpp +++ b/platforms/common/Serial.cpp @@ -130,4 +130,9 @@ bool Serial::setFlowcontrol(FlowControl flowcontrol) return _impl.setFlowcontrol(flowcontrol); } +const char *Serial::getPort() const +{ + return _impl.getPort(); +} + } // namespace device diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp index aa55e9a321..ce02b5ac63 100644 --- a/platforms/common/include/px4_platform_common/Serial.hpp +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -83,6 +83,8 @@ public: FlowControl getFlowcontrol() const; bool setFlowcontrol(FlowControl flowcontrol); + const char *getPort() const; + private: // Disable copy constructors Serial(const Serial &); diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index e8df9fcead..cfb207b174 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -325,24 +325,6 @@ const char *SerialImpl::getPort() const return _port; } -bool SerialImpl::setPort(const char *port) -{ - if (strcmp(port, _port) == 0) { - return true; - } - - strncpy(_port, port, sizeof(_port) - 1); - _port[sizeof(_port) - 1] = '\0'; - - // If old port is already opened then close it and reopen it on new port - if (_open) { - close(); - return open(); - } - - return true; -} - uint32_t SerialImpl::getBaudrate() const { return _baudrate; diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index 4476cb20a9..ec5e774c0a 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -65,7 +65,6 @@ 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); From 81ab314a4c359c83184951df82ceee155e87f402 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 20 Jul 2023 15:43:07 -0700 Subject: [PATCH 11/12] More cleanup and fixes from testing failures --- platforms/nuttx/src/px4/common/SerialImpl.cpp | 47 ++++----- .../src/px4/common/include/SerialImpl.hpp | 3 - platforms/posix/include/SerialImpl.hpp | 43 +++++--- platforms/posix/src/px4/common/SerialImpl.cpp | 97 ++++++++++--------- platforms/qurt/include/SerialImpl.hpp | 36 ++++++- platforms/qurt/src/px4/SerialImpl.cpp | 45 ++++++--- src/drivers/gps/gps.cpp | 2 + 7 files changed, 167 insertions(+), 106 deletions(-) diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index cfb207b174..11b3d3ce4d 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -60,7 +60,6 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P } else { _port[0] = 0; } - } SerialImpl::~SerialImpl() @@ -72,18 +71,14 @@ SerialImpl::~SerialImpl() bool SerialImpl::validateBaudrate(uint32_t baudrate) { - if ((baudrate == 9600) || - (baudrate == 19200) || - (baudrate == 38400) || - (baudrate == 57600) || - (baudrate == 115200) || - (baudrate == 230400) || - (baudrate == 460800) || - (baudrate == 921600)) { - return true; - } - - return false; + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); } bool SerialImpl::configure() @@ -131,7 +126,10 @@ bool SerialImpl::configure() int termios_state; /* fill the struct for the new configuration */ - tcgetattr(_serial_fd, &uart_config); + 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 ) */ @@ -202,15 +200,14 @@ bool SerialImpl::open() return false; } - // Configure the serial port if a baudrate has been configured - if (_baudrate) { - if (! configure()) { - PX4_ERR("failed to configure %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; } - _serial_fd = serial_fd; _open = true; return _open; @@ -245,10 +242,6 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) if (ret < 0) { PX4_DEBUG("%s read error %d", _port, ret); - - } else { - // Track total bytes read - _bytes_read += ret; } return ret; @@ -311,10 +304,6 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) if (written < 0) { PX4_ERR("%s write error %d", _port, written); - - } else { - // Track total bytes written - _bytes_written += written; } return written; diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index ec5e774c0a..58d41bf759 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -85,9 +85,6 @@ private: int _serial_fd{-1}; - size_t _bytes_read{0}; - size_t _bytes_written{0}; - bool _open{false}; char _port[32] {}; diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp index 93765efac7..efc95d7d51 100644 --- a/platforms/posix/include/SerialImpl.hpp +++ b/platforms/posix/include/SerialImpl.hpp @@ -1,4 +1,35 @@ - +/**************************************************************************** + * + * 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 @@ -34,7 +65,6 @@ 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); @@ -55,9 +85,6 @@ private: int _serial_fd{-1}; - size_t _bytes_read{0}; - size_t _bytes_written{0}; - bool _open{false}; char _port[32] {}; @@ -71,12 +98,6 @@ private: bool validateBaudrate(uint32_t baudrate); bool configure(); - - // 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 diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index ef76c2ebf7..c216e32656 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -1,7 +1,39 @@ +/**************************************************************************** + * + * 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 // strncpy +#include #include #include #include @@ -26,7 +58,6 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P } else { _port[0] = 0; } - } SerialImpl::~SerialImpl() @@ -38,18 +69,14 @@ SerialImpl::~SerialImpl() bool SerialImpl::validateBaudrate(uint32_t baudrate) { - if ((baudrate == 9600) || - (baudrate == 19200) || - (baudrate == 38400) || - (baudrate == 57600) || - (baudrate == 115200) || - (baudrate == 230400) || - (baudrate == 460800) || - (baudrate == 921600)) { - return true; - } - - return false; + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); } bool SerialImpl::configure() @@ -97,7 +124,10 @@ bool SerialImpl::configure() int termios_state; /* fill the struct for the new configuration */ - tcgetattr(_serial_fd, &uart_config); + 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 ) */ @@ -168,15 +198,14 @@ bool SerialImpl::open() return false; } - // Configure the serial port if a baudrate has been configured - if (_baudrate) { - if (! configure()) { - PX4_ERR("failed to configure %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; } - _serial_fd = serial_fd; _open = true; return _open; @@ -212,9 +241,6 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) if (ret < 0) { PX4_DEBUG("%s read error %d", _port, ret); - } else { - // Track total bytes read - _bytes_read += ret; } return ret; @@ -278,9 +304,6 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) if (written < 0) { PX4_ERR("%s write error %d", _port, written); - } else { - // Track total bytes written - _bytes_written += written; } return written; @@ -291,24 +314,6 @@ const char *SerialImpl::getPort() const return _port; } -bool SerialImpl::setPort(const char *port) -{ - if (strcmp(port, _port) == 0) { - return true; - } - - strncpy(_port, port, sizeof(_port) - 1); - _port[sizeof(_port) - 1] = '\0'; - - // If old port is already opened then close it and reopen it on new port - if (_open) { - close(); - return open(); - } - - return true; -} - uint32_t SerialImpl::getBaudrate() const { return _baudrate; diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp index dadda42e77..1b98d3bb40 100644 --- a/platforms/qurt/include/SerialImpl.hpp +++ b/platforms/qurt/include/SerialImpl.hpp @@ -1,4 +1,35 @@ - +/**************************************************************************** + * + * 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 @@ -54,9 +85,6 @@ private: int _serial_fd{-1}; - size_t _bytes_read{0}; - size_t _bytes_written{0}; - bool _open{false}; char _port[32] {}; diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index f4606a8046..ec0fb73fce 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -1,3 +1,35 @@ +/**************************************************************************** + * + * 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 @@ -132,8 +164,6 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) if (ret_read < 0) { PX4_DEBUG("%s read error %d", _port, ret_read); - } else { - _bytes_read += ret_read; } return ret_read; @@ -216,8 +246,6 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) if (ret_write < 0) { PX4_ERR("%s write error %d", _port, ret_write); - } else { - _bytes_written += ret_write; } return ret_write; @@ -228,15 +256,6 @@ const char *SerialImpl::getPort() const return _port; } -bool SerialImpl::setPort(const char *port) -{ - if (strcmp(port, _port) == 0) { - return true; - } - - return false; -} - uint32_t SerialImpl::getBaudrate() const { return _baudrate; diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 645b1d7737..ae92000bfc 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -795,7 +795,9 @@ GPS::run() px4_sleep(1); continue; } + } + if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) { // Configure the desired baudrate if one was specified by the user. // Otherwise the default baudrate will be used. if (_configured_baudrate) { From 936d7c4f28d68d3ad49723f9283e2cd0972f3707 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Wed, 26 Jul 2023 14:15:44 -0700 Subject: [PATCH 12/12] Fixed delete and non-null pointer bug --- src/drivers/gps/gps.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index ae92000bfc..431a871f04 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1031,6 +1031,7 @@ GPS::run() if ((_interface == GPSHelper::Interface::UART) && (_uart)) { (void) _uart->close(); delete _uart; + _uart = nullptr; #ifdef __PX4_LINUX