mixer_module: send a last sample out after all outputs were disabled

This matters for PWM when the last output gets disabled on either FMU or IO
it would just keep on running.

Also when rebooting with a parameters reset or new airframe with no mapped outputs
it would previously keep outputting PWM with the disarmed value of the new airframe
e.g. 1000us which is a safety hazard because servos could break the physical limit of the
model or miscalibrated ESCs spinning motors.

.editorconfig

@@ -0,0 +1,14 @@
+root = true
+
+[*]
+insert_final_newline = false
+
+[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
+indent_style = tab
+tab_width = 8
+# Not in the official standard, but supported by many editors
+max_line_length = 120
+
+[*.yaml]
+indent_style = space
+indent_size = 2

.gitmodules

@@ -81,5 +81,5 @@
 	url = https://github.com/PX4/PX4-gazebo-models.git
 	branch = main
 [submodule "boards/modalai/voxl2/libfc-sensor-api"]
-	path = src/modules/muorb/apps/libfc-sensor-api
+	path = boards/modalai/voxl2/libfc-sensor-api
 	url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git

Tools/astyle/files_to_check_code_style.sh

@@ -30,5 +30,5 @@ exec find boards msg src platforms test \
     -path src/lib/cdrstream/cyclonedds -prune -o \
     -path src/lib/cdrstream/rosidl -prune -o \
     -path src/modules/zenoh/zenoh-pico -prune -o \
-    -path src/modules/muorb/apps/libfc-sensor-api -prune -o \
+    -path boards/modalai/voxl2/libfc-sensor-api -prune -o \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

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

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

boards/modalai/voxl2/cmake/init.cmake

@@ -0,0 +1,34 @@
+############################################################################
+#
+#   Copyright (c) 2024 ModalAI, Inc. 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_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)

boards/modalai/voxl2/cmake/link_libraries.cmake

@@ -1,7 +1,7 @@
 
 # Link against the public stub version of the proprietary fc sensor library
 target_link_libraries(px4 PRIVATE
-		${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
+		${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
 		px4_layer
 		${module_libraries}
 )

boards/modalai/voxl2/scripts/build-deps.sh

@@ -1,10 +1,9 @@
 #!/bin/bash
 
-cd src/modules/muorb/apps/libfc-sensor-api
+cd boards/modalai/voxl2/libfc-sensor-api
 rm -fR build
 mkdir build
 cd build
 CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
 make
-cd ../../../../../..
-
+cd ../../../../..

boards/px4/fmu-v5x/default.px4board

@@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_PAYLOAD_DELIVERER=y
 CONFIG_MODULES_RC_UPDATE=y
-CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -102,7 +101,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y

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)

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 <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)
+{
+	// 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

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 <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);
+
+	// 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

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

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

platforms/common/uORB/SubscriptionBlocking.hpp

@@ -56,31 +56,6 @@ public:
 	SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
 		SubscriptionCallback(meta, interval_us, instance)
 	{
-		// pthread_mutexattr_init
-		pthread_mutexattr_t attr;
-		int ret_attr_init = pthread_mutexattr_init(&attr);
-
-		if (ret_attr_init != 0) {
-			PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
-		}
-
-#if defined(PTHREAD_PRIO_NONE)
-		// pthread_mutexattr_settype
-		//  PTHREAD_PRIO_NONE not available on cygwin
-		int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
-
-		if (ret_mutexattr_settype != 0) {
-			PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
-		}
-
-#endif // PTHREAD_PRIO_NONE
-
-		// pthread_mutex_init
-		int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
-
-		if (ret_mutex_init != 0) {
-			PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init);
-		}
 	}
 
 	virtual ~SubscriptionBlocking()

platforms/common/uORB/uORBDeviceNode.cpp

@@ -412,7 +412,10 @@ int16_t uORB::DeviceNode::process_add_subscription()
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
 
 	if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
-		ch->send_message(_meta->o_name, _meta->o_size, _data);
+		// Only send the most recent data to initialize the remote end.
+		if (_data_valid) {
+			ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue)));
+		}
 	}
 
 	return PX4_OK;

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 <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h>
+#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::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

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 <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;
+
+	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

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

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

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 <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;
+
+	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

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

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 <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h>
+#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::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

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

@@ -51,6 +51,11 @@
 #include <errno.h>
 #include "hrt_work.h"
 
+// Voxl2 board specific API definitions to get time offset
+#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+#include "fc_sensor.h"
+#endif
+
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <lockstep_scheduler/lockstep_scheduler.h>
 static LockstepScheduler lockstep_scheduler {true};
@@ -107,6 +112,29 @@ hrt_abstime hrt_absolute_time()
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	struct timespec ts;
 	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+
+# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+	hrt_abstime temp_abstime = ts_to_abstime(&ts);
+	int apps_time_offset = fc_sensor_get_time_offset();
+
+	if (apps_time_offset < 0) {
+		hrt_abstime temp_offset = -apps_time_offset;
+
+		if (temp_offset >= temp_abstime) {
+			temp_abstime = 0;
+
+		} else {
+			temp_abstime -= temp_offset;
+		}
+
+	} else {
+		temp_abstime += (hrt_abstime) apps_time_offset;
+	}
+
+	ts.tv_sec = temp_abstime / 1000000;
+	ts.tv_nsec = (temp_abstime % 1000000) * 1000;
+# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+
 	return ts_to_abstime(&ts);
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 }
@@ -449,6 +477,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	return system_clock_gettime(clk_id, tp);
+
 }
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)

platforms/qurt/CMakeLists.txt

@@ -50,5 +50,4 @@ add_library(px4 SHARED
 target_link_libraries(px4
 	modules__muorb__slpi
 	${module_libraries}
-	px4_layer
 	)

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 <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};
+
+	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

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,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 <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;
+	}
+}
+
+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

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;
 }

src/drivers/drv_hrt.h

@@ -162,7 +162,16 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
  */
 static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
 {
-	return hrt_absolute_time() - *then;
+	hrt_abstime now = hrt_absolute_time();
+
+	// Cannot allow a negative elapsed time as this would appear
+	// to be a huge positive elapsed time when represented as an
+	// unsigned value!
+	if (*then > now) {
+		return 0;
+	}
+
+	return now - *then;
 }
 
 /**

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,8 @@
 #include <px4_platform_common/cli.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/Serial.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
@@ -81,6 +82,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 +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) {
@@ -1406,7 +1387,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance)
 				   entry_point, (char *const *)argv);
 
 	if (task_id < 0) {
-		task_id = -1;
+		_task_id = -1;
 		return -errno;
 	}
 
@@ -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;

src/drivers/px4io/px4io.cpp

@@ -364,6 +364,13 @@ PX4IO::~PX4IO()
 bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
+	for (size_t i = 0; i < num_outputs; i++) {
+		if (!_mixing_output.isFunctionSet(i)) {
+			// do not run any signal on disabled channels
+			outputs[i] = 0;
+		}
+	}
+
 	if (!_test_fmu_fail) {
 		/* output to the servos */
 		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);

src/lib/mixer_module/mixer_module.cpp

@@ -455,7 +455,8 @@ bool MixingOutput::update()
 		}
 	}
 
-	if (!all_disabled) {
+	// Send output if any function mapped or one last disabling sample
+	if (!all_disabled || !_was_all_disabled) {
 		if (!_armed.armed && !_armed.manual_lockdown) {
 			_actuator_test.overrideValues(outputs, _max_num_outputs);
 		}
@@ -463,6 +464,8 @@ bool MixingOutput::update()
 		limitAndUpdateOutputs(outputs, has_updates);
 	}
 
+	_was_all_disabled = all_disabled;
+
 	return true;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -288,6 +288,7 @@ private:
 	hrt_abstime _lowrate_schedule_interval{300_ms};
 	ActuatorTest _actuator_test{_function_assignment};
 	uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only)
+	bool _was_all_disabled{false};
 
 	uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback
 

src/lib/mixer_module/mixer_module_tests.cpp

@@ -191,11 +191,15 @@ TEST_F(MixerModuleTest, basic)
 	mixing_output.setAllMaxValues(MAX_VALUE);
 	EXPECT_EQ(test_module.num_updates, 0);
 
-	// all functions disabled: not expected to get an update
+	// all functions disabled: expect to get one single update to process disabling the output signal
 	mixing_output.update();
 	mixing_output.updateSubscriptions(false);
 	mixing_output.update();
-	EXPECT_EQ(test_module.num_updates, 0);
+	EXPECT_EQ(test_module.num_updates, 1);
+	mixing_output.update();
+	mixing_output.updateSubscriptions(false);
+	mixing_output.update();
+	EXPECT_EQ(test_module.num_updates, 1);
 	test_module.reset();
 
 	// configure motor, ensure all still disarmed

src/modules/muorb/apps/CMakeLists.txt

@@ -39,7 +39,7 @@ px4_add_module(
 	INCLUDES
 		../test
 		../aggregator
-		libfc-sensor-api/inc
+		${PX4_BOARD_DIR}/libfc-sensor-api/inc
 	SRCS
 		uORBAppsProtobufChannel.cpp
 		muorb_main.cpp

src/modules/muorb/apps/Kconfig

@@ -4,3 +4,11 @@ menuconfig MODULES_MUORB_APPS
 	depends on PLATFORM_POSIX
 	---help---
 		Enable support for muorb apps
+
+
+    config MUORB_APPS_SYNC_TIMESTAMP
+        bool "Sync timestamp with external processor"
+        depends on MODULES_MUORB_APPS
+        default y
+        help
+            causes HRT timestamp to use an externally calculated offset for synchronization

src/modules/px4iofirmware/mixer.cpp

@@ -180,15 +180,19 @@ mixer_tick()
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
-		/* copy failsafe values to the servo outputs */
+		// Set failsafe value if the PWM output isn't disabled
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			r_page_servos[i] = r_page_servo_failsafe[i];
+			if (r_page_servos[i] != 0) {
+				r_page_servos[i] = r_page_servo_failsafe[i];
+			}
 		}
 
 	} else if (source == MIX_DISARMED) {
-		/* copy disarmed values to the servo outputs */
+		// Set disarmed value if the PWM output isn't disabled
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			r_page_servos[i] = r_page_servo_disarmed[i];
+			if (r_page_servos[i] != 0) {
+				r_page_servos[i] = r_page_servo_disarmed[i];
+			}
 		}
 	}