First working version. Tested on ModalAI FlightCore V2

platforms/common/CMakeLists.txt

@@ -51,9 +51,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)

platforms/common/Serial.cpp

@@ -0,0 +1,114 @@
+#include <px4_platform_common/Serial.hpp>
+
+namespace device
+{
+
+Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+	       FlowControl flowcontrol) :
+	_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
+{
+
+
+	// TODO: Device
+
+	// set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
+
+	// char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
+	// set_device_bus(c - 48); // sub 48 to convert char to integer
+}
+
+Serial::~Serial()
+{
+}
+
+bool Serial::open()
+{
+	return _impl.open();
+}
+
+bool Serial::isOpen() const
+{
+	return _impl.isOpen();
+}
+
+bool Serial::close()
+{
+	return _impl.close();
+}
+
+ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
+{
+	return _impl.read(buffer, buffer_size);
+}
+
+ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
+}
+
+ssize_t Serial::write(const void *buffer, size_t buffer_size)
+{
+	return _impl.write(buffer, buffer_size);
+}
+
+const char *Serial::getPort() const
+{
+	return _impl.getPort();
+}
+
+bool Serial::setPort(const char *port)
+{
+	return _impl.setPort(port);
+}
+
+uint32_t Serial::getBaudrate() const
+{
+	return _impl.getBaudrate();
+}
+
+bool Serial::setBaudrate(uint32_t baudrate)
+{
+	return _impl.setBaudrate(baudrate);
+}
+
+ByteSize Serial::getBytesize() const
+{
+	return _impl.getBytesize();
+}
+
+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

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 <SerialImpl.hpp>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device __EXPORT
+{
+
+class Serial
+{
+public:
+	Serial(const char *port, uint32_t baudrate = 57600,
+	       ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
+	       StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
+	virtual ~Serial();
+
+	// Open sets up the port and gets it configured based on desired configuration
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	// 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

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

platforms/nuttx/src/px4/common/SerialImpl.cpp

@@ -0,0 +1,367 @@
+
+#include <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h> // strncpy
+#include <px4_log.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+#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

platforms/nuttx/src/px4/common/include/SerialImpl.hpp

@@ -0,0 +1,81 @@
+
+
+#pragma once
+
+#include <stdint.h>
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	const char *getPort() const;
+	bool setPort(const char *port);
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	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

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

platforms/posix/include/SerialImpl.hpp

@@ -0,0 +1,81 @@
+
+
+#pragma once
+
+#include <stdint.h>
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	const char *getPort() const;
+	bool setPort(const char *port);
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	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

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

platforms/posix/src/px4/common/SerialImpl.cpp

@@ -0,0 +1,365 @@
+
+#include <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h> // strncpy
+#include <px4_log.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+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

platforms/qurt/include/SerialImpl.hpp

@@ -0,0 +1,78 @@
+
+
+#pragma once
+
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	const char *getPort() const;
+	bool setPort(const char *port);
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	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

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

platforms/qurt/src/px4/SerialImpl.cpp

@@ -0,0 +1,294 @@
+
+#include <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <px4_log.h>
+#include <drivers/device/qurt/uart.h>
+#include <drivers/drv_hrt.h>
+
+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

src/drivers/gps/gps.cpp

@@ -45,7 +45,6 @@
 #include <poll.h>
 #endif
 
-#include <termios.h>
 #include <cstring>
 
 #include <drivers/drv_sensor.h>
@@ -57,6 +56,7 @@
 #include <px4_platform_common/cli.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/Serial.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
@@ -81,6 +81,7 @@
 #include <linux/spi/spidev.h>
 #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:
+	case GPSCallbackType::writeDeviceData: {
-		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
+			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,
+		pollfd fds[1];
-	//so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
+		fds[0].fd = _spi_fd;
-	//two pollings use different underlying mechanisms (at least under posix), which makes this
+		fds[0].events = POLLIN;
-	//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];
+		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), _timeout);
-	fds[0].fd = _serial_fd;
-	fds[0].events = POLLIN;
 
-	int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
+		if (ret > 0) {
-
+			/* if we have new data from GPS, go handle it */
-	if (ret > 0) {
+			if (fds[0].revents & POLLIN) {
-		/* 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
-			 * We are here because poll says there is some data, so this
+				 * by 1-2 bytes, wait for some more data to save expensive read() calls.
-			 * won't block even on a blocking device. But don't read immediately
+				 * If we have all requested data available, read it without waiting.
-			 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+				 * If more bytes are available, we'll go back to poll() again.
-			 * 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);
-			const unsigned character_count = 32; // minimum bytes that we want to read
-			unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
-			const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
 
 #ifdef __PX4_NUTTX
-			int err = 0;
+				int err = 0;
-			int bytes_available = 0;
+				int bytes_available = 0;
-			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
+				err = ::ioctl(_spi_fd, FIONREAD, (unsigned long)&bytes_available);
-
-			if (err != 0 || bytes_available < (int)character_count) {
-				px4_usleep(sleeptime);
-			}
 
+				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) {
+				if (ret > 0) {
-				_num_bytes_read += ret;
+					_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);
+	size_t written = 0;
-	::fsync(_serial_fd);
+
+	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 */
+	if (_interface == GPSHelper::Interface::UART) {
-	int speed;
+		if (_uart) {
-
+			return _uart->setBaudrate(baud);
-	switch (baud) {
+		}
-	case 9600:   speed = B9600;   break;
+	} else if (_interface == GPSHelper::Interface::SPI) {
-
+		// Can't set the baudrate on a SPI port but just return a success
-	case 19200:  speed = B19200;  break;
+		return 0;
-
-	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;
 	}
 
-	struct termios uart_config;
+	return -1;
-
-	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;
 }
 
 void GPS::initializeCommunicationDump()
@@ -840,33 +779,39 @@ GPS::run()
 			_helper = nullptr;
 		}
 
-		if (_serial_fd < 0) {
+		if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
-			/* open the serial port */
+			_uart = new Serial(_port, _configured_baudrate);
-			_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
 
-			if (_serial_fd < 0) {
+			if (_uart == nullptr) {
-				PX4_ERR("failed to open %s err: %d", _port, errno);
+				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) {
+			if (status_value < 0) {
-				int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
+				PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-				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);
-				}
 			}
 
+			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) {
+		if ((_interface == GPSHelper::Interface::UART) && (_uart)){
-			::close(_serial_fd);
+			_uart->close();
-			_serial_fd = -1;
+			delete _uart;
-		}
+
+		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)){
+			::close(_spi_fd);
+			_spi_fd = -1;
+		} 
 
 		if (_mode_auto) {
 			switch (_mode) {