From 69028f37a91b48add269ef699101922e52410446 Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Thu, 21 Mar 2024 18:00:23 -0700 Subject: [PATCH] New platform independent Serial interface (#21723) --- boards/modalai/fc-v2/default.px4board | 2 +- boards/modalai/voxl2-slpi/default.px4board | 1 + platforms/common/CMakeLists.txt | 1 + platforms/common/Serial.cpp | 138 ++++++ .../include/px4_platform_common/Serial.hpp | 97 +++++ .../px4_platform_common/SerialCommon.hpp | 70 ++++ .../common/include/px4_platform_common/time.h | 13 +- platforms/nuttx/src/px4/common/SerialImpl.cpp | 394 ++++++++++++++++++ .../src/px4/common/include/SerialImpl.hpp | 104 +++++ .../nuttx/src/px4/common/px4_layer.cmake | 2 + .../src/px4/common/px4_protected_layers.cmake | 2 + platforms/posix/include/SerialImpl.hpp | 103 +++++ platforms/posix/src/px4/common/CMakeLists.txt | 2 + platforms/posix/src/px4/common/SerialImpl.cpp | 387 +++++++++++++++++ platforms/qurt/CMakeLists.txt | 1 - platforms/qurt/include/SerialImpl.hpp | 108 +++++ platforms/qurt/src/px4/CMakeLists.txt | 1 + platforms/qurt/src/px4/SerialImpl.cpp | 326 +++++++++++++++ platforms/qurt/src/px4/drv_hrt.cpp | 2 +- src/drivers/gps/gps.cpp | 327 +++++++-------- 20 files changed, 1903 insertions(+), 178 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/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index be01cfa934..c4e7d36495 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index 73e75dd7bc..b6ce6133fc 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y +CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index a0bafe7d59..cb0721fe69 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(px4_platform STATIC shutdown.cpp spi.cpp pab_manifest.c + Serial.cpp ${SRCS} ) target_link_libraries(px4_platform prebuild_targets px4_work_queue) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp new file mode 100644 index 0000000000..2f93a66a6e --- /dev/null +++ b/platforms/common/Serial.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +namespace device +{ + +Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _impl(port, baudrate, bytesize, parity, stopbits, flowcontrol) +{ + // If no baudrate was specified then set it to a reasonable default value + if (baudrate == 0) { + (void) _impl.setBaudrate(9600); + } +} + +Serial::~Serial() +{ +} + +bool Serial::open() +{ + return _impl.open(); +} + +bool Serial::isOpen() const +{ + return _impl.isOpen(); +} + +bool Serial::close() +{ + return _impl.close(); +} + +ssize_t Serial::read(uint8_t *buffer, size_t buffer_size) +{ + return _impl.read(buffer, buffer_size); +} + +ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us); +} + +ssize_t Serial::write(const void *buffer, size_t buffer_size) +{ + return _impl.write(buffer, buffer_size); +} + +uint32_t Serial::getBaudrate() const +{ + return _impl.getBaudrate(); +} + +bool Serial::setBaudrate(uint32_t baudrate) +{ + return _impl.setBaudrate(baudrate); +} + +ByteSize Serial::getBytesize() const +{ + return _impl.getBytesize(); +} + +bool Serial::setBytesize(ByteSize bytesize) +{ + return _impl.setBytesize(bytesize); +} + +Parity Serial::getParity() const +{ + return _impl.getParity(); +} + +bool Serial::setParity(Parity parity) +{ + return _impl.setParity(parity); +} + +StopBits Serial::getStopbits() const +{ + return _impl.getStopbits(); +} + +bool Serial::setStopbits(StopBits stopbits) +{ + return _impl.setStopbits(stopbits); +} + +FlowControl Serial::getFlowcontrol() const +{ + return _impl.getFlowcontrol(); +} + +bool Serial::setFlowcontrol(FlowControl flowcontrol) +{ + return _impl.setFlowcontrol(flowcontrol); +} + +const char *Serial::getPort() const +{ + return _impl.getPort(); +} + +} // namespace device diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp new file mode 100644 index 0000000000..ce02b5ac63 --- /dev/null +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device __EXPORT +{ + +class Serial +{ +public: + Serial(const char *port, uint32_t baudrate = 57600, + ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None, + StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled); + virtual ~Serial(); + + // Open sets up the port and gets it configured based on desired configuration + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + // If port is already open then the following configuration functions + // will reconfigure the port. If the port is not yet open then they will + // simply store the configuration in preparation for the port to be opened. + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + + const char *getPort() const; + +private: + // Disable copy constructors + Serial(const Serial &); + Serial &operator=(const Serial &); + + // platform implementation + SerialImpl _impl; +}; + +} // namespace device diff --git a/platforms/common/include/px4_platform_common/SerialCommon.hpp b/platforms/common/include/px4_platform_common/SerialCommon.hpp new file mode 100644 index 0000000000..bbe9be0ad9 --- /dev/null +++ b/platforms/common/include/px4_platform_common/SerialCommon.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +namespace device +{ +namespace SerialConfig +{ + + +// ByteSize: number of data bits +enum class ByteSize { + FiveBits = 5, + SixBits = 6, + SevenBits = 7, + EightBits = 8, +}; + +// Parity: enable parity checking +enum class Parity { + None = 0, + Odd = 1, + Even = 2, +}; + +// StopBits: number of stop bits +enum class StopBits { + One = 1, + Two = 2 +}; + +// FlowControl: enable flow control +enum class FlowControl { + Disabled = 0, + Enabled = 1, +}; + +} // namespace SerialConfig +} // namespace device diff --git a/platforms/common/include/px4_platform_common/time.h b/platforms/common/include/px4_platform_common/time.h index 1b4d8a2756..d61e23c75c 100644 --- a/platforms/common/include/px4_platform_common/time.h +++ b/platforms/common/include/px4_platform_common/time.h @@ -13,11 +13,21 @@ __END_DECLS #define px4_clock_gettime system_clock_gettime #endif -#if defined(ENABLE_LOCKSTEP_SCHEDULER) +#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT) __BEGIN_DECLS __EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp); +__END_DECLS +#else + +#define px4_clock_settime system_clock_settime + +#endif + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + +__BEGIN_DECLS __EXPORT int px4_usleep(useconds_t usec); __EXPORT unsigned int px4_sleep(unsigned int seconds); __EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond, @@ -27,7 +37,6 @@ __END_DECLS #else -#define px4_clock_settime system_clock_settime #define px4_usleep system_usleep #define px4_sleep system_sleep #define px4_pthread_cond_timedwait system_pthread_cond_timedwait diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp new file mode 100644 index 0000000000..7490dd604c --- /dev/null +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -0,0 +1,394 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include +#include +#include +#include + +#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::validateBaudrate(uint32_t baudrate) +{ + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); +} + +bool SerialImpl::configure() +{ + /* process baud rate */ + int speed; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("ERR: unknown baudrate: %lu", _baudrate); + return false; + } + + switch (_baudrate) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + +#ifndef B460800 +#define B460800 460800 +#endif + + case 460800: speed = B460800; break; + +#ifndef B921600 +#define B921600 921600 +#endif + + case 921600: speed = B921600; break; + + default: + PX4_ERR("ERR: unknown baudrate: %lu", _baudrate); + return false; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcgetattr)", termios_state); + return false; + } + + /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return false; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return false; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + if (isOpen()) { + return true; + } + + // Open the serial port + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + return false; + } + + _serial_fd = serial_fd; + + // Configure the serial port + if (! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + + if (_serial_fd >= 0) { + ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret = ::read(_serial_fd, buffer, buffer_size); + + if (ret < 0) { + PX4_DEBUG("%s read error %d", _port, ret); + } + + return ret; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) { + // Poll for incoming UART data. + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); + + if (remaining_time <= 0) { break; } + + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); + + if (ret > 0) { + if (fds[0].revents & POLLIN) { + const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10); + + 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); + } + + ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (ret > 0) { + total_bytes_read += ret; + } + + } else { + PX4_ERR("Got a poll error"); + return -1; + } + } + } + + return total_bytes_read; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int written = ::write(_serial_fd, buffer, buffer_size); + ::fsync(_serial_fd); + + if (written < 0) { + PX4_ERR("%s write error %d", _port, written); + } + + return written; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("ERR: invalid baudrate: %lu", baudrate); + return false; + } + + // check if already configured + if ((baudrate == _baudrate) && _open) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return configure(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // namespace device diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp new file mode 100644 index 0000000000..58d41bf759 --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + const char *getPort() const; + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + bool configure(); + +}; + +} // namespace device diff --git a/platforms/nuttx/src/px4/common/px4_layer.cmake b/platforms/nuttx/src/px4/common/px4_layer.cmake index 69fd1a007d..9199a95289 100644 --- a/platforms/nuttx/src/px4/common/px4_layer.cmake +++ b/platforms/nuttx/src/px4/common/px4_layer.cmake @@ -3,6 +3,8 @@ add_library(px4_layer ${KERNEL_SRCS} cdc_acm_check.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.cpp + SerialImpl.cpp ) target_link_libraries(px4_layer diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 9c57b488b6..a1c6ebc391 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -15,6 +15,8 @@ add_library(px4_layer usr_board_ctrl.c usr_hrt.cpp usr_mcu_version.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.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..efc95d7d51 --- /dev/null +++ b/platforms/posix/include/SerialImpl.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + const char *getPort() const; + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + bool configure(); +}; + +} // namespace device diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 34b65cdf44..90d4a77992 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -46,6 +46,8 @@ add_library(px4_layer drv_hrt.cpp cpuload.cpp print_load.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.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..79e3422aed --- /dev/null +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -0,0 +1,387 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + return ((baudrate == 9600) || + (baudrate == 19200) || + (baudrate == 38400) || + (baudrate == 57600) || + (baudrate == 115200) || + (baudrate == 230400) || + (baudrate == 460800) || + (baudrate == 921600)); +} + +bool SerialImpl::configure() +{ + /* process baud rate */ + int speed; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("ERR: unknown baudrate: %u", _baudrate); + return false; + } + + switch (_baudrate) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + +#ifndef B460800 +#define B460800 460800 +#endif + + case 460800: speed = B460800; break; + +#ifndef B921600 +#define B921600 921600 +#endif + + case 921600: speed = B921600; break; + + default: + PX4_ERR("ERR: unknown baudrate: %d", _baudrate); + return false; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcgetattr)", termios_state); + return false; + } + + /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return false; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return false; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + if (isOpen()) { + return true; + } + + // Open the serial port + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + return false; + } + + _serial_fd = serial_fd; + + // Configure the serial port + if (! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + + if (_serial_fd >= 0) { + ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret = ::read(_serial_fd, buffer, buffer_size); + + if (ret < 0) { + PX4_DEBUG("%s read error %d", _port, ret); + + } + + return ret; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) { + // Poll for incoming UART data. + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); + + if (remaining_time <= 0) { break; } + + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); + + if (ret > 0) { + if (fds[0].revents & POLLIN) { + const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10); + px4_usleep(sleeptime); + + ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (ret > 0) { + total_bytes_read += ret; + } + + } else { + PX4_ERR("Got a poll error"); + return -1; + } + } + } + + return total_bytes_read; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int written = ::write(_serial_fd, buffer, buffer_size); + ::fsync(_serial_fd); + + if (written < 0) { + PX4_ERR("%s write error %d", _port, written); + + } + + return written; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("ERR: invalid baudrate: %u", baudrate); + return false; + } + + // check if already configured + if ((baudrate == _baudrate) && _open) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return configure(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // namespace device diff --git a/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 ) diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp new file mode 100644 index 0000000000..1b98d3bb40 --- /dev/null +++ b/platforms/qurt/include/SerialImpl.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + const char *getPort() const; + bool setPort(const char *port); + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + + // Mutex used to lock the read functions + //pthread_mutex_t read_mutex; + + // Mutex used to lock the write functions + //pthread_mutex_t write_mutex; +}; + +} // namespace device 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..ec0fb73fce --- /dev/null +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -0,0 +1,326 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (port) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + if ((baudrate != 9600) && + (baudrate != 38400) && + (baudrate != 57600) && + (baudrate != 115200) && + (baudrate != 230400) && + (baudrate != 250000) && + (baudrate != 420000) && + (baudrate != 460800) && + (baudrate != 921600) && + (baudrate != 1000000) && + (baudrate != 1843200) && + (baudrate != 2000000)) { + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + // There's no harm in calling open multiple times on the same port. + // In fact, that's the only way to change the baudrate + + _open = false; + _serial_fd = -1; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("Invalid baudrate: %u", _baudrate); + return false; + } + + if (_bytesize != ByteSize::EightBits) { + PX4_ERR("Qurt platform only supports ByteSize::EightBits"); + return false; + } + + if (_parity != Parity::None) { + PX4_ERR("Qurt platform only supports Parity::None"); + return false; + } + + if (_stopbits != StopBits::One) { + PX4_ERR("Qurt platform only supports StopBits::One"); + return false; + } + + if (_flowcontrol != FlowControl::Disabled) { + PX4_ERR("Qurt platform only supports FlowControl::Disabled"); + return false; + } + + // qurt_uart_open will check validity of port and baudrate + int serial_fd = qurt_uart_open(_port, _baudrate); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd); + return false; + + } else { + PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate); + } + + _serial_fd = serial_fd; + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + // No close defined for qurt uart yet + // if (_serial_fd >= 0) { + // qurt_uart_close(_serial_fd); + // } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500); + + if (ret_read < 0) { + PX4_DEBUG("%s read error %d", _port, ret_read); + + } + + return ret_read; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + int total_bytes_read = 0; + + while (total_bytes_read < (int) character_count) { + + if (timeout_us > 0) { + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + + if (elapsed_us >= timeout_us) { + // If there was a partial read but not enough to satisfy the minimum then they will be lost + // but this really should never happen when everything is working normally. + // PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us); + // Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)? + return total_bytes_read; + } + } + + int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (current_bytes_read < 0) { + // Again, if there was a partial read but not enough to satisfy the minimum then they will be lost + // but this really should never happen when everything is working normally. + PX4_ERR("%s failed to read uart", __FUNCTION__); + // Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)? + return -1; + } + + // Current bytes read could be zero + total_bytes_read += current_bytes_read; + + // If we have at least reached our desired minimum number of characters + // then we can return now + if (total_bytes_read >= (int) character_count) { + return total_bytes_read; + } + + // Wait a set amount of time before trying again or the remaining time + // until the timeout if we are getting close + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + int64_t time_until_timeout = timeout_us - elapsed_us; + uint64_t time_to_sleep = 5000; + + if ((time_until_timeout >= 0) && + (time_until_timeout < (int64_t) time_to_sleep)) { + time_to_sleep = time_until_timeout; + } + + px4_usleep(time_to_sleep); + } + + return -1; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size); + + if (ret_write < 0) { + PX4_ERR("%s write error %d", _port, ret_write); + + } + + return ret_write; +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("Invalid baudrate: %u", baudrate); + return false; + } + + // check if already configured + if (baudrate == _baudrate) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return open(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +} // namespace device diff --git a/platforms/qurt/src/px4/drv_hrt.cpp b/platforms/qurt/src/px4/drv_hrt.cpp index 8bb9546a01..2ef1520832 100644 --- a/platforms/qurt/src/px4/drv_hrt.cpp +++ b/platforms/qurt/src/px4/drv_hrt.cpp @@ -81,7 +81,7 @@ static void hrt_unlock() px4_sem_post(&_hrt_lock); } -int px4_clock_settime(clockid_t clk_id, struct timespec *tp) +int px4_clock_settime(clockid_t clk_id, const struct timespec *tp) { return 0; } diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fcbd5a0c83..766be323d7 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,8 @@ #include #include #include +#include +#include #include #include #include @@ -81,6 +82,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 +171,10 @@ public: void reset_if_scheduled(); private: - int _serial_fd{-1}; ///< serial 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) char _port[20] {}; ///< device / serial port path @@ -329,8 +334,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) { @@ -403,10 +411,23 @@ 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); + +#ifdef __PX4_LINUX + + } else if (gps->_spi_fd >= 0) { + ret = ::write(gps->_spi_fd, data1, (size_t)data2); +#endif + } + + return ret; + } case GPSCallbackType::setBaudrate: return gps->setBaudrate(data2); @@ -449,72 +470,64 @@ 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_adjusted = math::min(max_timeout, timeout); + handleInjectDataTopic(); -#if !defined(__PX4_QURT) + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted); - /* For non QURT, use the usual polling. */ +// SPI is only supported on LInux +#if defined(__PX4_LINUX) - //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; + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; + //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 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. + //FIXME: add a unified poll() API - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout)); + pollfd fds[1]; + fds[0].fd = _spi_fd; + fds[0].events = POLLIN; - 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); + ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted); -#ifdef __PX4_NUTTX - int err = 0; - int bytes_available = 0; - err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + 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); - if (err != 0 || bytes_available < (int)character_count) { px4_usleep(sleeptime); + + ret = ::read(_spi_fd, buf, buf_length); + + if (ret > 0) { + _num_bytes_read += ret; + } + + } else { + ret = -1; } - -#else - px4_usleep(sleeptime); -#endif - - ret = ::read(_serial_fd, buf, buf_length); - - if (ret > 0) { - _num_bytes_read += ret; - } - - } 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,38 @@ 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); + +#ifdef __PX4_LINUX + + } else if (_interface == GPSHelper::Interface::SPI) { + written = ::write(_spi_fd, data, len); + ::fsync(_spi_fd); +#endif + } + return written == len; } int GPS::setBaudrate(unsigned baud) { - /* process baud rate */ - int speed; + if (_interface == GPSHelper::Interface::UART) { + if ((_uart) && (_uart->setBaudrate(baud))) { + return 0; + } - switch (baud) { - case 9600: speed = B9600; break; +#ifdef __PX4_LINUX - 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 + } else if (_interface == GPSHelper::Interface::SPI) { + // Can't set the baudrate on a SPI port but just return a success + return 0; #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; } - 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,31 +786,58 @@ 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)) { - if (_serial_fd < 0) { - PX4_ERR("failed to open %s err: %d", _port, errno); + // Create the UART port instance + _uart = new Serial(_port); + + if (_uart == nullptr) { + PX4_ERR("Error creating serial device %s", _port); + 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) { + 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 - 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); + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) { + _spi_fd = ::open(_port, O_RDWR | O_NOCTTY); - if (status_value < 0) { - PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } + if (_spi_fd < 0) { + PX4_ERR("failed to open SPI port %s err: %d", _port, errno); + px4_sleep(1); + continue; + } - status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); + 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 (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,9 +1029,17 @@ GPS::run() } } - if (_serial_fd >= 0) { - ::close(_serial_fd); - _serial_fd = -1; + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + (void) _uart->close(); + delete _uart; + _uart = nullptr; + +#ifdef __PX4_LINUX + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { + ::close(_spi_fd); + _spi_fd = -1; +#endif } if (_mode_auto) { @@ -1477,12 +1458,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; @@ -1490,12 +1471,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;