atune: paremeterize intial RLS variance

Rearrange to look like a KF

boards/modalai/voxl2/default.px4board

@@ -3,7 +3,6 @@ CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_BOARD_ROOTFSDIR="/data/px4"
 CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
-CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_QSHELL_POSIX=y
 CONFIG_MODULES_COMMANDER=y

boards/px4/fmu-v6xrt/src/CMakeLists.txt

@@ -36,7 +36,6 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "bootloader")
 		bootloader_main.c
 		init.c
 		usb.c
-		imxrt_romapi.c
 		imxrt_flexspi_nor_boot.c
 		imxrt_flexspi_nor_flash.c
 		imxrt_clockconfig.c
@@ -48,6 +47,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "bootloader")
 			nuttx_drivers # sdio
 			px4_layer #gpio
 			arch_io_pins # iotimer
+			arch_board_romapi
 			bootloader
 	)
 	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common)
@@ -72,7 +72,6 @@ else()
 		spi.cpp
 		timer_config.cpp
 		usb.c
-		imxrt_romapi.c
 		imxrt_flexspi_fram.c
 		imxrt_flexspi_nor_boot.c
 		imxrt_flexspi_nor_flash.c
@@ -83,6 +82,7 @@ else()
 	target_link_libraries(drivers_board
 		PRIVATE
 			arch_board_hw_info
+			arch_board_romapi
 			arch_spi
 			drivers__led # drv_led_start
 			nuttx_arch # sdio

boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c

@@ -22,7 +22,7 @@
  * Included Files
  ****************************************************************************/
 
-#include "imxrt_flexspi_nor_flash.h"
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
 
 /****************************************************************************
  * Public Data

boards/px4/fmu-v6xrt/src/init.c

@@ -66,8 +66,7 @@
 
 #include "arm_internal.h"
 #include "imxrt_flexspi_nor_boot.h"
-#include "imxrt_flexspi_nor_flash.h"
-#include "imxrt_romapi.h"
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
 #include "imxrt_iomuxc.h"
 #include "imxrt_flexcan.h"
 #include "imxrt_enet.h"
@@ -79,10 +78,12 @@
 
 #include <arch/board/board.h>
 
+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_board_led.h>
 #include <systemlib/px4_macros.h>
 #include <px4_arch/io_timer.h>
+#include <px4_arch/imxrt_romapi.h>
 #include <px4_platform_common/init.h>
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_determine_hw_info.h>

platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c

@@ -7,8 +7,8 @@
 #include <px4_defines.h>
 
 #include "hw_config.h"
-#include "imxrt_flexspi_nor_flash.h"
-#include "imxrt_romapi.h"
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
+#include <px4_arch/imxrt_romapi.h>
 #include <hardware/rt117x/imxrt117x_ocotp.h>
 #include <hardware/rt117x/imxrt117x_anadig.h>
 #include <hardware/rt117x/imxrt117x_snvs.h>

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/imxrt_flexspi_nor_flash.h

@@ -1,5 +1,5 @@
 /****************************************************************************
- * boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.h
+ * platforms/nuttx/src/px4/nxp/imrt/include/px4_arch/imxrt_flexspi_nor_flash.h
  *
  * Licensed to the Apache Software Foundation (ASF) under one or more
  * contributor license agreements.  See the NOTICE file distributed with
@@ -18,8 +18,8 @@
  *
  ****************************************************************************/
 
-#ifndef __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H
-#define __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H
+#ifndef __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H
+#define __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H
 
 /****************************************************************************
  * Included Files
@@ -349,4 +349,4 @@ extern const struct flexspi_nor_config_s g_flash_config;
 extern const struct flexspi_nor_config_s g_flash_fast_config;
 
 
-#endif /* __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H */
+#endif /* __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H */

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/imxrt_romapi.h

@@ -1,14 +1,14 @@
 /****************************************************************************
- * boards/px4/fmu-v6xrt/src/imxrt_romapi.c
+ * platforms/nuttx/src/px4/nxp/imrt/include/px4_arch/imxrt_romapi.h
  *
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2024 NXP
  * All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
 ****************************************************************************/
-#ifndef __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H
-#define __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H
+#ifndef __PX4_ARCH_IMXRT_ROMAPI_H
+#define __PX4_ARCH_IMXRT_ROMAPI_H
 
 /****************************************************************************
  *
@@ -370,4 +370,8 @@ void ROM_FLEXSPI_NorFlash_ClearCache(uint32_t instance);
 
 /*@}*/
 
-#endif /* __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H */
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __PX4_ARCH_IMXRT_ROMAPI_H */

platforms/nuttx/src/px4/nxp/imxrt/romapi/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2019 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.
+#
+############################################################################
+
+px4_add_library(arch_board_romapi
+	imxrt_romapi.c
+)

platforms/nuttx/src/px4/nxp/imxrt/romapi/imxrt_romapi.c

@@ -1,7 +1,7 @@
 /****************************************************************************
- * boards/px4/fmu-v6xrt/src/imxrt_romapi.c
+ * platforms/nuttx/src/px4/nxp/imrt/romapi/imxrt_romapi.c
  *
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2024 NXP
  * All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -22,8 +22,8 @@
 
 #include "arm_internal.h"
 
-#include "imxrt_flexspi_nor_flash.h"
-#include "imxrt_romapi.h"
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
+#include <px4_arch/imxrt_romapi.h>
 
 #include <hardware/rt117x/imxrt117x_anadig.h>
 

platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt

@@ -36,7 +36,7 @@ add_subdirectory(adc)
 add_subdirectory(../imxrt/board_critmon board_critmon)
 add_subdirectory(../imxrt/board_hw_info board_hw_info)
 add_subdirectory(../imxrt/board_reset board_reset)
-#add_subdirectory(../imxrt/dshot dshot)
+add_subdirectory(../imxrt/romapi romapi)
 add_subdirectory(../imxrt/hrt hrt)
 add_subdirectory(../imxrt/led_pwm led_pwm)
 add_subdirectory(../imxrt/io_pins io_pins)

platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/imxrt_flexspi_nor_flash.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2024 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
@@ -30,24 +30,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 #pragma once
 
-#include <stdint.h>
 
-namespace device
-{
-
-class SerialSBUSImpl
-{
-public:
-
-	SerialSBUSImpl() {};
-	virtual ~SerialSBUSImpl() {};
-
-	bool configure(int fd, uint32_t baud);
-
-	static const uint32_t DEFAULT_BAUDRATE{100000};
-};
-
-} // namespace device
+#include "../../../imxrt/include/px4_arch/imxrt_flexspi_nor_flash.h"

platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/imxrt_romapi.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2024 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
@@ -30,26 +30,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 #pragma once
 
-#include <stdint.h>
 
-namespace device
-{
-
-class SerialStandardImpl
-{
-public:
-
-	SerialStandardImpl() {};
-	virtual ~SerialStandardImpl() {};
-
-	bool configure(int fd, uint32_t baud);
-
-private:
-	bool validateBaudrate(uint32_t baudrate);
-
-};
-
-} // namespace device
+#include "../../../imxrt/include/px4_arch/imxrt_romapi.h"

src/drivers/gps/CMakeLists.txt

@@ -56,5 +56,4 @@ px4_add_module(
 		module.yaml
 	DEPENDS
 		git_gps_devices
-		drivers__device
 	)

src/drivers/gps/gps.cpp

@@ -45,11 +45,11 @@
 #include <poll.h>
 #endif
 
+#include <termios.h>
 #include <cstring>
 
 #include <drivers/drv_sensor.h>
 #include <lib/drivers/device/Device.hpp>
-#include <lib/drivers/device/Serial.hpp>
 #include <lib/parameters/param.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
@@ -81,7 +81,6 @@
 #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
@@ -170,10 +169,7 @@ public:
 	void reset_if_scheduled();
 
 private:
-#ifdef __PX4_LINUX
-	int				_spi_fd {-1};					///< SPI interface to GPS
-#endif
-	Serial 			*_uart = nullptr;	
+	int				_serial_fd{-1};					///< serial 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
@@ -333,10 +329,8 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
 		set_device_bus(c - 48); // sub 48 to convert char to integer
 
-#ifdef __PX4_LINUX
 	} else if (_interface == GPSHelper::Interface::SPI) {
 		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
-#endif
 	}
 
 	if (_mode == gps_driver_mode_t::None) {
@@ -409,22 +403,10 @@ 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);
 
-			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;
-		}
+		return ::write(gps->_serial_fd, data1, (size_t)data2);
 
 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -455,11 +437,10 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 			// as of 2021 setting the time on Nuttx temporarily pauses interrupts
 			// so only set the time if it is very wrong.
 			// TODO: clock slewing of the RTC for small time differences
-#ifndef __PX4_QURT
 			px4_clock_settime(CLOCK_REALTIME, &rtc_gps_time);
-#endif
 		}
 
+
 		break;
 	}
 
@@ -468,64 +449,72 @@ 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 ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-		ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
+#if !defined(__PX4_QURT)
 
-// SPI is only supported on Linux
-#if defined(__PX4_LINUX)
+	/* For non QURT, use the usual polling. */
 
-	} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
+	//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;
 
-		//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
+	pollfd fds[1];
+	fds[0].fd = _serial_fd;
+	fds[0].events = POLLIN;
 
-		pollfd fds[1];
-		fds[0].fd = _spi_fd;
-		fds[0].events = POLLIN;
+	int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
 
-		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
+	if (ret > 0) {
+		/* if we have new data from GPS, go handle it */
+		if (fds[0].revents & POLLIN) {
+			/*
+			 * We are here because poll says there is some data, so this
+			 * won't block even on a blocking device. But don't read immediately
+			 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+			 * If we have all requested data available, read it without waiting.
+			 * If more bytes are available, we'll go back to poll() again.
+			 */
+			const unsigned character_count = 32; // minimum bytes that we want to read
+			unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
+			const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
 
-		if (ret > 0) {
-			/* if we have new data from GPS, go handle it */
-			if (fds[0].revents & POLLIN) {
-				/*
-				 * We are here because poll says there is some data, so this
-				 * won't block even on a blocking device. But don't read immediately
-				 * by 1-2 bytes, wait for some more data to save expensive read() calls.
-				 * If we have all requested data available, read it without waiting.
-				 * If more bytes are available, we'll go back to poll() again.
-				 */
-				unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
-				const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
+#ifdef __PX4_NUTTX
+			int err = 0;
+			int bytes_available = 0;
+			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
 
+			if (err != 0 || bytes_available < (int)character_count) {
 				px4_usleep(sleeptime);
-
-				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;
+		}
 	}
 
 	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()
@@ -594,38 +583,105 @@ bool GPS::injectData(uint8_t *data, size_t len)
 {
 	dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
 
-	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
-	}
-
+	size_t written = ::write(_serial_fd, data, len);
+	::fsync(_serial_fd);
 	return written == len;
 }
 
 int GPS::setBaudrate(unsigned baud)
 {
-	if (_interface == GPSHelper::Interface::UART) {
-		if ((_uart) && (_uart->setBaudrate(baud))) {
-			return 0;
-		}
+	/* process baud rate */
+	int speed;
 
-#ifdef __PX4_LINUX
+	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
-		return 0;
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
 #endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %d", baud);
+		return -EINVAL;
 	}
 
-	return -1;
+	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;
 }
 
 void GPS::initializeCommunicationDump()
@@ -784,58 +840,31 @@ GPS::run()
 			_helper = nullptr;
 		}
 
-		if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
+		if (_serial_fd < 0) {
+			/* open the serial port */
+			_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
 
-			// Create the UART port instance
-			_uart = new Serial(_port);
-
-			if (_uart == nullptr) {
-				PX4_ERR("Error creating serial device %s", _port);
-				px4_usleep(100000);
-				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_usleep(100000);
-					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_usleep(100000);
+			if (_serial_fd < 0) {
+				PX4_ERR("failed to open %s err: %d", _port, errno);
+				px4_sleep(1);
 				continue;
 			}
 
 #ifdef __PX4_LINUX
 
-		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
-			_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
+			if (_interface == GPSHelper::Interface::SPI) {
+				int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
+				int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
 
-			if (_spi_fd < 0) {
-				PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
-				px4_sleep(1);
-				continue;
-			}
+				if (status_value < 0) {
+					PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				}
 
-			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);
+				status_value = ::ioctl(_serial_fd, SPI_IOC_RD_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(_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);
+				if (status_value < 0) {
+					PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				}
 			}
 
 #endif /* __PX4_LINUX */
@@ -1027,17 +1056,9 @@ GPS::run()
 			}
 		}
 
-		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 (_serial_fd >= 0) {
+			::close(_serial_fd);
+			_serial_fd = -1;
 		}
 
 		if (_mode_auto) {
@@ -1456,12 +1477,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'i':
-			if (!strcmp(myoptarg, "uart")) {
-				interface = GPSHelper::Interface::UART;
-#ifdef __PX4_LINUX
-			} else if (!strcmp(myoptarg, "spi")) {
+			if (!strcmp(myoptarg, "spi")) {
 				interface = GPSHelper::Interface::SPI;
-#endif
+
+			} else if (!strcmp(myoptarg, "uart")) {
+				interface = GPSHelper::Interface::UART;
+
 			} else {
 				PX4_ERR("unknown interface: %s", myoptarg);
 				error_flag = true;
@@ -1469,12 +1490,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'j':
-			if (!strcmp(myoptarg, "uart")) {
-				interface_secondary = GPSHelper::Interface::UART;
-#ifdef __PX4_LINUX
-			} else if (!strcmp(myoptarg, "spi")) {
+			if (!strcmp(myoptarg, "spi")) {
 				interface_secondary = GPSHelper::Interface::SPI;
-#endif
+
+			} else if (!strcmp(myoptarg, "uart")) {
+				interface_secondary = GPSHelper::Interface::UART;
+
 			} else {
 				PX4_ERR("unknown interface for secondary: %s", myoptarg);
 				error_flag = true;

src/lib/drivers/device/CMakeLists.txt

@@ -40,31 +40,20 @@ if (${PX4_PLATFORM} STREQUAL "nuttx")
 	if ("${CONFIG_SPI}" STREQUAL "y")
 		list(APPEND SRCS_PLATFORM nuttx/SPI.cpp)
 	endif()
-
-	list(APPEND SRCS_PLATFORM nuttx/SerialImpl.cpp)
 elseif((${PX4_PLATFORM} MATCHES "qurt"))
 	list(APPEND SRCS_PLATFORM qurt/I2C.cpp)
 	list(APPEND SRCS_PLATFORM qurt/SPI.cpp)
-	list(APPEND SRCS_PLATFORM qurt/SerialImpl.cpp)
 	list(APPEND SRCS_PLATFORM qurt/uart.c)
 elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type
 	# Linux I2Cdev and SPIdev
-	if ("${CONFIG_I2C}" STREQUAL "y")
-		list(APPEND SRCS_PLATFORM posix/I2C.cpp)
-	endif()
-
-	if ("${CONFIG_SPI}" STREQUAL "y")
-		list(APPEND SRCS_PLATFORM posix/SPI.cpp)
-	endif()
-
-	list(APPEND SRCS_PLATFORM posix/SerialImpl.cpp)
-	list(APPEND SRCS_PLATFORM posix/SerialSBUSImpl.cpp)
-	list(APPEND SRCS_PLATFORM posix/SerialStandardImpl.cpp)
+	list(APPEND SRCS_PLATFORM
+		posix/I2C.cpp
+		posix/SPI.cpp
+	)
 endif()
 
 px4_add_library(drivers__device
 	CDev.cpp
-	Serial.cpp
 	${SRCS_PLATFORM}
 	)
 

src/lib/drivers/device/Serial.cpp

@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   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 "Serial.hpp"
-
-namespace device
-{
-
-Serial::Serial(const char *port, uint32_t baudrate) :
-	_impl(port, baudrate)
-{
-}
-
-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);
-}
-
-bool Serial::getSBUSMode() const
-{
-	return _impl.getSBUSMode();
-}
-
-bool Serial::setSBUSMode(bool enable)
-{
-	return _impl.setSBUSMode(enable);
-}
-
-const char *Serial::getPort() const
-{
-	return _impl.getPort();
-}
-
-} // namespace device

src/lib/drivers/device/Serial.hpp

@@ -1,89 +0,0 @@
-/****************************************************************************
- *
- *   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
-
-// Bring in the correct platform implementation
-#ifdef __PX4_NUTTX
-#include "nuttx/SerialImpl.hpp"
-#elif defined(__PX4_QURT)
-#include "qurt/SerialImpl.hpp"
-#else
-#include "posix/SerialImpl.hpp"
-#endif
-
-namespace device __EXPORT
-{
-
-class Serial
-{
-public:
-	// Baud rate can be selected with constructor or by using setBaudrate
-	Serial(const char *port, uint32_t baudrate = 0);
-	virtual ~Serial();
-
-	// Open sets up the port and gets it configured. Unless an alternate mode
-	// is selected the port will be configured with parity disabled and 1 stop bit.
-	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);
-
-	uint32_t getBaudrate() const;
-
-	// If the port has already been opened it will be reconfigured with a change
-	// of baudrate.
-	bool setBaudrate(uint32_t baudrate);
-
-	// SBUS has special configuration considerations and methods so it
-	// is given a special mode. It has parity enabled and 2 stop bits
-	bool getSBUSMode() const;
-	bool setSBUSMode(bool enable);
-
-	const char *getPort() const;
-
-private:
-	// Disable copy constructors
-	Serial(const Serial &);
-	Serial &operator=(const Serial &);
-
-	// platform implementation
-	SerialImpl _impl;
-};
-
-} // namespace device

src/lib/drivers/device/nuttx/SerialImpl.cpp

@@ -1,363 +0,0 @@
-/****************************************************************************
- *
- *   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>
-#include <board_config.h>
-
-namespace device
-{
-
-SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-
- 	if (baudrate) {
-		_baudrate = baudrate;
-	} else {
-		// If baudrate is zero then choose a reasonable default
-		_baudrate = 9600;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	if (isOpen()) {
-		close();
-	}
-}
-
-bool SerialImpl::configure()
-{
-	struct termios uart_config;
-
-	if (_SBUSMode) {
-		if (_baudrate != DEFAULT_SBUS_BAUDRATE) {
-			PX4_WARN("Warning, SBUS baud rate being set to %lu", _baudrate);
-		}
-
-		/* even parity, two stop bits */
-		tcgetattr(_serial_fd, &uart_config);
-		cfsetspeed(&uart_config, _baudrate);
-		uart_config.c_cflag |= (CSTOPB | PARENB);
-		tcsetattr(_serial_fd, TCSANOW, &uart_config);
-
-		if (board_rc_singlewire(_port)) {
-			/* only defined in configs capable of IOCTL
-			 * Note It is never turned off
-			 */
-#ifdef TIOCSSINGLEWIRE
-			ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
-#endif
-		}
-	} else {
-		/* process baud rate */
-		int speed;
-
-		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;
-		}
-
-		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);
-		close();
-		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) {
-				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)
-{
-	// check if already configured
-	if (baudrate == _baudrate) {
-		return true;
-	}
-
-	_baudrate = baudrate;
-
-	// process baud rate change now if port is already open
-	if ((_open) && (configure() != 0)) {
-		// Configure failed! Close the port
-		close();
-		return false;
-	}
-
-	return true;
-}
-
-bool SerialImpl::getSBUSMode() const
-{
-	return _SBUSMode;
-}
-
-bool SerialImpl::setSBUSMode(bool enable)
-{
-	if (_open) {
-		PX4_ERR("Cannot configure SBUS mode after port has already been opened");
-		return false;
-	}
-
-	_SBUSMode = enable;
-	_baudrate = DEFAULT_SBUS_BAUDRATE;
-
-	return true;
-}
-
-} // namespace device

src/lib/drivers/device/posix/SerialImpl.cpp

@@ -1,253 +0,0 @@
-/****************************************************************************
- *
- *   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 <unistd.h>
-
-#include <string.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)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-
- 	if (baudrate) {
-		_baudrate = baudrate;
-	} else {
-		// If baudrate is zero then choose a reasonable default
-		_baudrate = 9600;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	if (isOpen()) {
-		close();
-	}
-}
-
-bool SerialImpl::configure()
-{
-	if (_SBUSMode) {
-		return _sbus.configure(_serial_fd, _baudrate);
-	}
-
-	return _standard.configure(_serial_fd, _baudrate);
-}
-
-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);
-		close();
-		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) {
-				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)
-{
-	// check if already configured
-	if (baudrate == _baudrate) {
-		return true;
-	}
-
-	_baudrate = baudrate;
-
-	// process baud rate change now if port is already open
-	if ((_open) && (configure() != 0)) {
-		// Configure failed! Close the port
-		close();
-		return false;
-	}
-
-	return true;
-}
-
-bool SerialImpl::getSBUSMode() const
-{
-	return _SBUSMode;
-}
-
-bool SerialImpl::setSBUSMode(bool enable)
-{
-	if (_open) {
-		PX4_ERR("Cannot configure SBUS mode after port has already been opened");
-		return false;
-	}
-
-	_SBUSMode = enable;
-	_baudrate = SerialSBUSImpl::DEFAULT_BAUDRATE;
-
-	return true;
-}
-
-} // namespace device

src/lib/drivers/device/posix/SerialImpl.hpp

@@ -1,89 +0,0 @@
-/****************************************************************************
- *
- *   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 "SerialSBUSImpl.hpp"
-#include "SerialStandardImpl.hpp"
-
-namespace device
-{
-
-class SerialImpl
-{
-public:
-
-	SerialImpl(const char *port, uint32_t baudrate);
-	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);
-
-	bool getSBUSMode() const;
-	bool setSBUSMode(bool enable);
-
-private:
-
-	int _serial_fd{-1};
-
-	bool _open{false};
-
-	char _port[32] {};
-
-	uint32_t _baudrate{0};
-
-	bool _SBUSMode{false};
-
-	// The configuration routines for SBUS versus other needed to be separated
-	// out because they use different methods with different, conflicting header files
-	SerialSBUSImpl _sbus;
-	SerialStandardImpl _standard;
-
-	bool configure();
-};
-
-} // namespace device

src/lib/drivers/device/posix/SerialSBUSImpl.cpp

@@ -1,80 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SerialSBUSImpl.hpp"
-
-#include <sys/ioctl.h>
-#include <asm-generic/termbits.h>
-#include <px4_log.h>
-
-namespace device
-{
-
-bool SerialSBUSImpl::configure(int fd, uint32_t baud)
-{
-	struct termios2 tio = {};
-
-	if (ioctl(fd, TCGETS2, &tio)) {
-		return false;
-	}
-
-	if (baud != DEFAULT_BAUDRATE) {
-		PX4_WARN("Warning, SBUS baud rate being set to %u", baud);
-	}
-
-	/**
-	 * Setting serial port,8E2, non-blocking.100Kbps
-	 */
-	tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL
-			 | IXON);
-	tio.c_iflag |= (INPCK | IGNPAR);
-	tio.c_oflag &= ~OPOST;
-	tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
-	tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
-	/**
-	 * use BOTHER to specify speed directly in c_[io]speed member
-	 */
-	tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);
-	tio.c_ispeed = baud;
-	tio.c_ospeed = baud;
-	tio.c_cc[VMIN] = 25;
-	tio.c_cc[VTIME] = 0;
-
-	if (ioctl(fd, TCSETS2, &tio)) {
-		return false;
-	}
-
-	return true;
-}
-
-} // namespace device

src/lib/drivers/device/posix/SerialStandardImpl.cpp

@@ -1,159 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SerialStandardImpl.hpp"
-
-#include <sys/termios.h>
-#include <px4_log.h>
-
-namespace device
-{
-
-bool SerialStandardImpl::validateBaudrate(uint32_t baudrate)
-{
-	return ((baudrate == 9600) ||
-		(baudrate == 19200) ||
-		(baudrate == 38400) ||
-		(baudrate == 57600) ||
-		(baudrate == 115200) ||
-		(baudrate == 230400) ||
-		(baudrate == 460800) ||
-		(baudrate == 921600));
-}
-
-bool SerialStandardImpl::configure(int fd, uint32_t baud)
-{
-	/* process baud rate */
-	int speed;
-
-	if (! validateBaudrate(baud)) {
-		PX4_ERR("ERR: unknown baudrate: %u", baud);
-		return false;
-	}
-
-	switch (baud) {
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-#ifndef B460800
-#define B460800 460800
-#endif
-
-	case 460800: speed = B460800; break;
-
-#ifndef B921600
-#define B921600 921600
-#endif
-
-	case 921600: speed = B921600; break;
-
-	default:
-		PX4_ERR("ERR: unknown baudrate: %d", baud);
-		return false;
-	}
-
-	struct termios uart_config;
-
-	int termios_state;
-
-	/* fill the struct for the new configuration */
-	if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
-		return false;
-	}
-
-	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
-
-	//
-	// Input flags - Turn off input processing
-	//
-	// convert break to null byte, no CR to NL translation,
-	// no NL to CR translation, don't mark parity errors or breaks
-	// no input parity check, don't strip high bit off,
-	// no XON/XOFF software flow control
-	//
-	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
-				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
-
-	//
-	// Output flags - Turn off output processing
-	//
-	// no CR to NL translation, no NL to CR-NL translation,
-	// no NL to CR translation, no column 0 CR suppression,
-	// no Ctrl-D suppression, no fill characters, no case mapping,
-	// no local output processing
-	//
-	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
-	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
-	uart_config.c_oflag = 0;
-
-	//
-	// No line processing
-	//
-	// echo off, echo newline off, canonical mode off,
-	// extended input processing off, signal chars off
-	//
-	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-
-	/* no parity, one stop bit, disable flow control */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
-
-	/* set baud rate */
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
-		return false;
-	}
-
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
-		return false;
-	}
-
-	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
-		return false;
-	}
-
-	return true;
-}
-
-} // namespace device

src/lib/drivers/device/qurt/SerialImpl.cpp

@@ -1,239 +0,0 @@
-/****************************************************************************
- *
- *   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)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-
- 	if (baudrate) {
-		_baudrate = baudrate;
-	} else {
-		// If baudrate is zero then choose a reasonable default.
-		// The default is the GPS UBX M10 default rate.
-		_baudrate = 115200;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	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;
-
-	// 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 at baudrate %u, fd: %d", _port, _baudrate, serial_fd);
-		close();
-		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)
-{
-	// 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;
-}
-
-} // namespace device

src/lib/drivers/device/qurt/SerialImpl.hpp

@@ -1,80 +0,0 @@
-/****************************************************************************
- *
- *   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>
-
-namespace device
-{
-
-class SerialImpl
-{
-public:
-
-	SerialImpl(const char *port, uint32_t baudrate);
-	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);
-
-	// Cannot configure Qurt UARTs for SBUS!
-	bool getSBUSMode() const { return false; }
-	bool setSBUSMode(bool enable) { return false; }
-
-private:
-
-	int _serial_fd{-1};
-
-	bool _open{false};
-
-	char _port[32] {};
-
-	uint32_t _baudrate{0};
-
-	bool validateBaudrate(uint32_t baudrate);
-};
-
-} // namespace device

src/lib/system_identification/arx_rls.hpp

@@ -84,13 +84,13 @@ public:
 	float getInnovation() const { return _innovation; }
 	const matrix::Vector < float, N + M + 1 > &getDiffEstimate() const { return _diff_theta_hat; }
 
-	void reset(const matrix::Vector < float, N + M + 1 > &theta_init = {})
+	void reset(const matrix::Vector < float, N + M + 1 > &theta_init = {}, const float var_init = 100.f)
 	{
 		/* _P.uncorrelateCovarianceSetVariance<N + M + 1>(0, 10e3f); // does not work */
 		_P.setZero();
 
 		for (size_t i = 0; i < (N + M + 1); i++) {
-			_P(i, i) = 10e3f;
+			_P(i, i) = var_init;
 		}
 
 		_diff_theta_hat.setZero();
@@ -115,18 +115,14 @@ public:
 
 		addInputOutput(u, y);
 
-		if (!isBufferFull()) {
-			// Do not start to update the RLS algorithm when the
-			// buffer still contains zeros
-			return;
-		}
-
 		const matrix::Vector < float, N + M + 1 > phi = constructDesignVector();
 		const matrix::Matrix < float, 1, N + M + 1 > phi_t = phi.transpose();
 
-		_P = (_P - _P * phi * phi_t * _P / (_lambda + (phi_t * _P * phi)(0, 0))) / _lambda;
+		auto K = _P * phi / ((phi_t * _P * phi)(0, 0) + _lambda);
 		_innovation = _y[N] - (phi_t * _theta_hat)(0, 0);
-		_theta_hat = _theta_hat + _P * phi * _innovation;
+
+		_theta_hat = _theta_hat + K * _innovation;
+		_P = (_P - K * (phi_t * _P)) / _lambda;
 
 		for (size_t i = 0; i < N + M + 1; i++) {
 			_diff_theta_hat(i) = fabsf(_theta_hat(i) - theta_prev(i));
@@ -135,16 +131,11 @@ public:
 		/* fixCovarianceErrors(); // TODO: this could help against ill-conditioned matrix but needs more testing*/
 	}
 
-private:
 	void addInputOutput(float u, float y)
 	{
 		shiftRegisters();
 		_u[M + D] = u;
 		_y[N] = y;
-
-		if (!isBufferFull()) {
-			_nb_samples++;
-		}
 	}
 
 	void shiftRegisters()
@@ -158,8 +149,6 @@ private:
 		}
 	}
 
-	bool isBufferFull() const { return _nb_samples > (M + N + D); }
-
 	matrix::Vector < float, N + M + 1 > constructDesignVector() const
 	{
 		matrix::Vector < float, N + M + 1 > phi;

src/lib/system_identification/signal_generator.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,53 +31,45 @@
  *
  ****************************************************************************/
 
+/**
+ * @file signal_generator.hpp
+ */
+
 #pragma once
 
-#include <stdint.h>
-
-namespace device
+namespace signal_generator
 {
 
-class SerialImpl
+float getLinearSineSweep(float f_start, float f_end, float duration, float t)
 {
-public:
+	if (t > duration) {
+		return 0.f;
+	}
 
-	SerialImpl(const char *port, uint32_t baudrate);
-	virtual ~SerialImpl();
+	const float w_start = f_start * M_TWOPI_F;
+	const float w_end = f_end * M_TWOPI_F;
 
-	bool open();
-	bool isOpen() const;
+	return sinf(w_start * t + 0.5f * (w_end - w_start) * t * t / duration);
+}
 
-	bool close();
+float getLogSineSweep(float f_start, float f_end, float duration, float t)
+{
+	if (t > duration) {
+		return 0.f;
 
-	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);
+	float w_start = f_start * M_TWOPI_F;
+	float w_end = f_end * M_TWOPI_F;
 
-	const char *getPort() const;
+	if (f_start > f_end) {
+		// Handle high-to-low sweep correctly
+		w_start = f_end * M_TWOPI_F;
+		w_end = f_start * M_TWOPI_F;
+		t = duration - t;
+	}
 
-	uint32_t getBaudrate() const;
-	bool setBaudrate(uint32_t baudrate);
+	return sinf(t * powf(10.f, log10f(w_start) + (log10f(w_end) - log10f(w_start)) * t / duration));
+}
 
-	bool getSBUSMode() const;
-	bool setSBUSMode(bool enable);
-
-private:
-
-	int _serial_fd{-1};
-
-	bool _open{false};
-
-	char _port[32] {};
-
-	uint32_t _baudrate{0};
-
-	bool _SBUSMode{false};
-	static const uint32_t DEFAULT_SBUS_BAUDRATE{100000};
-
-	bool configure();
-
-};
-
-} // namespace device
+} /* namespace signal_generator */

src/lib/system_identification/system_identification.cpp

@@ -39,9 +39,9 @@
 
 #include "system_identification.hpp"
 
-void SystemIdentification::reset(const matrix::Vector<float, 5> &id_state_init)
+void SystemIdentification::reset(const matrix::Vector<float, _kParameters> &id_state_init, const float var_init)
 {
-	_rls.reset(id_state_init);
+	_rls.reset(id_state_init, var_init);
 	_u_lpf.reset(0.f);
 	_u_lpf.reset(0.f);
 	_u_hpf = 0.f;
@@ -79,8 +79,8 @@ void SystemIdentification::updateFilters(float u, float y)
 
 	const float u_lpf = _u_lpf.apply(u);
 	const float y_lpf = _y_lpf.apply(y);
-	_u_hpf = _alpha_hpf * _u_hpf + _alpha_hpf * (u_lpf - _u_prev);
-	_y_hpf = _alpha_hpf * _y_hpf + _alpha_hpf * (y_lpf - _y_prev);
+	_u_hpf = u_lpf - _u_prev - (_gamma_hpf - 1.f) * _u_hpf;
+	_y_hpf = y_lpf - _y_prev - (_gamma_hpf - 1.f) * _y_hpf;
 
 	_u_prev = u_lpf;
 	_y_prev = y_lpf;
@@ -88,10 +88,10 @@ void SystemIdentification::updateFilters(float u, float y)
 
 void SystemIdentification::updateFitness()
 {
-	const matrix::Vector<float, 5> &diff = _rls.getDiffEstimate();
+	const matrix::Vector<float, _kParameters> &diff = _rls.getDiffEstimate();
 	float sum = 0.f;
 
-	for (size_t i = 0; i < 5; i++) {
+	for (size_t i = 0; i < _kParameters; i++) {
 		sum += diff(i);
 	}
 

src/lib/system_identification/system_identification.hpp

@@ -50,18 +50,23 @@
 class SystemIdentification final
 {
 public:
+	static constexpr int _kPoles = 2;
+	static constexpr int _kZeros = 2;
+	static constexpr int _kDelays = 1;
+	static constexpr int _kParameters = _kPoles + _kZeros + 1;
+
 	SystemIdentification() = default;
 	~SystemIdentification() = default;
 
-	void reset(const matrix::Vector<float, 5> &id_state_init = {});
+	void reset(const matrix::Vector<float, _kParameters> &id_state_init = {}, float var_init = 100.f);
 	void update(float u, float y); // update filters and model
 	void update(); // update model only (to be called after updateFilters)
 	void updateFilters(float u, float y);
 	bool areFiltersInitialized() const { return _are_filters_initialized; }
 	void updateFitness();
-	const matrix::Vector<float, 5> &getCoefficients() const { return _rls.getCoefficients(); }
-	const matrix::Vector<float, 5> getVariances() const { return _rls.getVariances(); }
-	const matrix::Vector<float, 5> &getDiffEstimate() const { return _rls.getDiffEstimate(); }
+	const matrix::Vector<float, _kParameters> getCoefficients() const { return  _rls.getCoefficients(); }
+	const matrix::Vector<float, _kParameters> getVariances() const { return _rls.getVariances(); }
+	const matrix::Vector<float, _kParameters> getDiffEstimate() const { return _rls.getDiffEstimate(); }
 	float getFitness() const { return _fitness_lpf.getState(); }
 	float getInnovation() const { return _rls.getInnovation(); }
 
@@ -70,7 +75,7 @@ public:
 		_u_lpf.set_cutoff_frequency(sample_freq, cutoff);
 		_y_lpf.set_cutoff_frequency(sample_freq, cutoff);
 	}
-	void setHpfCutoffFrequency(float sample_freq, float cutoff) { _alpha_hpf = sample_freq / (sample_freq + 2.f * M_PI_F * cutoff); }
+	void setHpfCutoffFrequency(float sample_freq, float cutoff) { _gamma_hpf = tanf(M_PI_F * cutoff / sample_freq); }
 
 	void setForgettingFactor(float time_constant, float dt) { _rls.setForgettingFactor(time_constant, dt); }
 	void setFitnessLpfTimeConstant(float time_constant, float dt)
@@ -83,12 +88,12 @@ public:
 	float getFilteredOutputData() const { return _y_hpf; }
 
 private:
-	ArxRls<2, 2, 1> _rls;
+	ArxRls<_kPoles, _kZeros, _kDelays> _rls;
 	math::LowPassFilter2p<float> _u_lpf{400.f, 30.f};
 	math::LowPassFilter2p<float> _y_lpf{400.f, 30.f};
 
 	//TODO: replace by HighPassFilter class
-	float _alpha_hpf{0.f};
+	float _gamma_hpf{0.f};
 	float _u_hpf{0.f};
 	float _y_hpf{0.f};
 

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -169,14 +169,22 @@ void FwAutotuneAttitudeControl::Run()
 		const hrt_abstime now = hrt_absolute_time();
 		updateStateMachine(now);
 
-		Vector<float, 5> coeff = _sys_id.getCoefficients();
-		coeff(2) *= _input_scale;
-		coeff(3) *= _input_scale;
-		coeff(4) *= _input_scale;
+		Vector<float, SystemIdentification::_kParameters> coeff = _sys_id.getCoefficients();
 
-		const Vector3f num(coeff(2), coeff(3), coeff(4));
-		const Vector3f den(1.f, coeff(0), coeff(1));
-		_kiff(2) = (1.f + coeff(0) + coeff(1)) / (coeff(2) + coeff(3) + coeff(4)); // inverse of the static gain
+		Vector3f den(1.f, 0.f, 0.f);
+
+		for (int i = 0; i < SystemIdentification::_kPoles; i++) {
+			den(i + 1) = coeff(i);
+		}
+
+		Vector3f num;
+
+		for (int i = 0; i < SystemIdentification::_kZeros + 1; i++) {
+			coeff(SystemIdentification::_kPoles + i) *= _input_scale;
+			num(i) = coeff(SystemIdentification::_kPoles + i);
+		}
+
+		_kiff(2) = (den(0) + den(1) + den(2)) / (num(0) + num(1) + num(2)); // inverse of the static gain
 		const Vector3f num_design = num * _kiff(2); // PID algorithm design works better with systems having unit static gain
 		Vector3f kid = pid_design::computePidGmvc(num_design, den, _sample_interval_avg, 0.2f, 0.f, 0.4f);
 		_kiff(0) = kid(0);
@@ -187,7 +195,7 @@ void FwAutotuneAttitudeControl::Run()
 		// or K_att * (K_rate + K_ff) * rad(60) = 1
 		_attitude_p = math::constrain(1.f / (math::radians(60.f) * (_kiff(0) + _kiff(2))), 1.f, 5.f);
 
-		const Vector<float, 5> &coeff_var = _sys_id.getVariances();
+		const Vector<float, SystemIdentification::_kParameters> &coeff_var = _sys_id.getVariances();
 		const Vector3f rate_sp = _sys_id.areFiltersInitialized()
 					 ? getIdentificationSignal()
 					 : Vector3f();
@@ -291,8 +299,9 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 	// when identifying an axis, check if the estimate has converged
 	const float converged_thr = 1.f;
 
-	const float temp[5] = {0.f, 0.f, 0.f, 0.f, 0.f};
-	const Vector<float, 5> sys_id_init(temp);
+	Vector<float, 5> sys_id_init;
+	sys_id_init(0) = -1.5f;
+	sys_id_init(1) = 0.5f;
 
 	switch (_state) {
 	case state::idle:

src/modules/logger/logged_topics.cpp

@@ -50,7 +50,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("actuator_controls_status_0", 300);
 	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
-	add_optional_topic("autotune_attitude_control_status", 100);
+	add_optional_topic("autotune_attitude_control_status");
 	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
 	add_topic("cellular_status", 200);
@@ -391,6 +391,7 @@ void LoggedTopics::add_system_identification_topics()
 	add_topic("sensor_combined");
 	add_topic("vehicle_angular_velocity");
 	add_topic("vehicle_torque_setpoint");
+	add_optional_topic("autotune_attitude_control_status");
 }
 
 void LoggedTopics::add_mavlink_tunnel()

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp

@@ -61,8 +61,6 @@ bool McAutotuneAttitudeControl::init()
 		return false;
 	}
 
-	_signal_filter.setParameters(_publishing_dt_s, .2f); // runs in the slow publishing loop
-
 	return true;
 }
 
@@ -91,9 +89,7 @@ void McAutotuneAttitudeControl::Run()
 		updateStateMachine(hrt_absolute_time());
 	}
 
-	// new control data needed every iteration
-	if (_state == state::idle
-	    || !_vehicle_torque_setpoint_sub.updated()) {
+	if (_state == state::idle) {
 		return;
 	}
 
@@ -113,17 +109,23 @@ void McAutotuneAttitudeControl::Run()
 		}
 	}
 
-	vehicle_torque_setpoint_s vehicle_torque_setpoint;
 	vehicle_angular_velocity_s angular_velocity;
 
-	if (!_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint)
-	    || !_vehicle_angular_velocity_sub.copy(&angular_velocity)) {
+	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
+		_angular_velocity(0) = angular_velocity.xyz[0];
+		_angular_velocity(1) = angular_velocity.xyz[1];
+		_angular_velocity(2) = angular_velocity.xyz[2];
+	}
+
+	vehicle_torque_setpoint_s torque_setpoint;
+
+	if (!_vehicle_torque_setpoint_sub.update(&torque_setpoint)) {
 		return;
 	}
 
 	perf_begin(_cycle_perf);
 
-	const hrt_abstime timestamp_sample = vehicle_torque_setpoint.timestamp;
+	const hrt_abstime timestamp_sample = torque_setpoint.timestamp;
 
 	// collect sample interval average for filters
 	if (_last_run > 0) {
@@ -143,16 +145,16 @@ void McAutotuneAttitudeControl::Run()
 
 	// Send data to the filters at maximum frequency
 	if (_state == state::roll) {
-		_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[0],
-				      angular_velocity.xyz[0]);
+		_sys_id.updateFilters(torque_setpoint.xyz[0],
+				      _angular_velocity(0));
 
 	} else if (_state == state::pitch) {
-		_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[1],
-				      angular_velocity.xyz[1]);
+		_sys_id.updateFilters(torque_setpoint.xyz[1],
+				      _angular_velocity(1));
 
 	} else if (_state == state::yaw) {
-		_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[2],
-				      angular_velocity.xyz[2]);
+		_sys_id.updateFilters(torque_setpoint.xyz[2],
+				      _angular_velocity(2));
 	}
 
 	// Update the model at a lower frequency
@@ -167,17 +169,23 @@ void McAutotuneAttitudeControl::Run()
 		_model_update_counter = 0;
 	}
 
-	if (hrt_elapsed_time(&_last_publish) > _publishing_dt_hrt || _last_publish == 0) {
+	if (hrt_elapsed_time(&_last_publish) > (_publishing_dt_s * 1_s) || _last_publish == 0) {
 		const hrt_abstime now = hrt_absolute_time();
 		updateStateMachine(now);
 
-		Vector<float, 5> coeff = _sys_id.getCoefficients();
-		coeff(2) *= _input_scale;
-		coeff(3) *= _input_scale;
-		coeff(4) *= _input_scale;
+		Vector<float, SystemIdentification::_kParameters> coeff = _sys_id.getCoefficients();
 
-		const Vector3f num(coeff(2), coeff(3), coeff(4));
-		const Vector3f den(1.f, coeff(0), coeff(1));
+		Vector3f den(1.f, 0.f, 0.f);
+
+		for (int i = 0; i < SystemIdentification::_kPoles; i++) {
+			den(i + 1) = coeff(i);
+		}
+
+		Vector3f num;
+
+		for (int i = 0; i < SystemIdentification::_kZeros + 1; i++) {
+			num(i) = coeff(SystemIdentification::_kPoles + i);
+		}
 
 		const float model_dt = static_cast<float>(_model_update_scaler) * _filter_dt;
 
@@ -190,15 +198,20 @@ void McAutotuneAttitudeControl::Run()
 			_kid(2) = 0.f;
 		}
 
+		// Do not use derivative on the yaw axis as it often only amplifies noise
+		if ((_state == state::yaw) || (_state == state::yaw_pause)) {
+			_kid(2) = 0.f;
+		}
+
 		// To compute the attitude gain, use the following empirical rule:
 		// "An error of 60 degrees should produce the maximum control output"
 		// or K_att * K_rate * rad(60) = 1
 		_attitude_p = math::constrain(1.f / (math::radians(60.f) * _kid(0)), 2.f, 6.5f);
 
-		const Vector<float, 5> &coeff_var = _sys_id.getVariances();
+		const Vector<float, SystemIdentification::_kParameters> &coeff_var = _sys_id.getVariances();
 
 		const Vector3f rate_sp = _sys_id.areFiltersInitialized()
-					 ? getIdentificationSignal()
+					 ? getIdentificationSignal(now)
 					 : Vector3f();
 
 		autotune_attitude_control_status_s status{};
@@ -226,38 +239,33 @@ void McAutotuneAttitudeControl::Run()
 
 void McAutotuneAttitudeControl::checkFilters()
 {
-	if (_interval_count > 1000) {
-		// calculate sensor update rate
-		_sample_interval_avg = _interval_sum / _interval_count;
+	if (_interval_count < 1000) {
+		return;
+	}
 
-		// check if sample rate error is greater than 1%
-		bool reset_filters = false;
+	// calculate sensor update rate
+	const float sample_interval = _interval_sum / static_cast<float>(_interval_count);
 
-		if ((fabsf(_filter_dt - _sample_interval_avg) / _filter_dt) > 0.01f) {
-			reset_filters = true;
-		}
+	// check if sample rate error is greater than 1%
+	const bool reset_filters = (fabsf(_filter_dt - sample_interval) / _filter_dt) > 0.01f;
 
-		if (reset_filters || !_are_filters_initialized) {
-			_filter_dt = _sample_interval_avg;
+	if (reset_filters || !_are_filters_initialized) {
+		_filter_dt = sample_interval;
 
-			const float filter_rate_hz = 1.f / _filter_dt;
+		const float filter_rate_hz = 1.f / _filter_dt;
 
-			_sys_id.setLpfCutoffFrequency(filter_rate_hz, _param_imu_gyro_cutoff.get());
-			_sys_id.setHpfCutoffFrequency(filter_rate_hz, .5f);
+		_sys_id.setLpfCutoffFrequency(filter_rate_hz, 30.f);
+		_sys_id.setHpfCutoffFrequency(filter_rate_hz, .1f);
 
-			// Set the model sampling time depending on the gyro cutoff frequency
-			// as this is a good indicator of the maximum control loop bandwidth
-			float model_dt = math::constrain(math::max(1.f / (2.f * _param_imu_gyro_cutoff.get()), _filter_dt), _model_dt_min,
-							 _model_dt_max);
+		// Make sure the model dt is a integer multiple of the data update rate
+		float model_dt = 0.01f;
+		_model_update_scaler = math::max(int(model_dt / _filter_dt), 1);
+		model_dt = _model_update_scaler * _filter_dt;
 
-			_model_update_scaler = math::max(int(model_dt / _filter_dt), 1);
-			model_dt = _model_update_scaler * _filter_dt;
+		_sys_id.setForgettingFactor(60.f, model_dt);
+		_sys_id.setFitnessLpfTimeConstant(1.f, model_dt);
 
-			_sys_id.setForgettingFactor(60.f, model_dt);
-			_sys_id.setFitnessLpfTimeConstant(1.f, model_dt);
-
-			_are_filters_initialized = true;
-		}
+		_are_filters_initialized = true;
 
 		// reset sample interval accumulator
 		_last_run = 0;
@@ -266,8 +274,9 @@ void McAutotuneAttitudeControl::checkFilters()
 
 void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 {
-	// when identifying an axis, check if the estimate has converged
-	const float converged_thr = 50.f;
+	Vector<float, 5> state_init;
+	state_init(0) = -1.5f;
+	state_init(1) = 0.5f;
 
 	switch (_state) {
 	case state::idle:
@@ -288,21 +297,15 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if (_are_filters_initialized) {
 			_state = state::roll;
 			_state_start_time = now;
-			_sys_id.reset();
-			// first step needs to be shorter to keep the drone centered
-			_steps_counter = 5;
-			_max_steps = 10;
-			_signal_sign = 1;
-			_input_scale = 1.f / (_param_mc_rollrate_p.get() * _param_mc_rollrate_k.get());
-			_signal_filter.reset(0.f);
+			_sys_id.reset(state_init, _kInitVar);
 			_gains_backup_available = false;
 		}
 
 		break;
 
 	case state::roll:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
-		    && ((now - _state_start_time) > 5_s)) {
+		if ((_sys_id.getVariances().max() < _kInitVar)
+		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(0);
 
 			// wait for the drone to stabilize
@@ -316,20 +319,14 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if ((now - _state_start_time) > 2_s) {
 			_state = state::pitch;
 			_state_start_time = now;
-			_sys_id.reset();
-			_input_scale = 1.f / (_param_mc_pitchrate_p.get() * _param_mc_pitchrate_k.get());
-			_signal_filter.reset(0.f);
-			_signal_sign = 1;
-			// first step needs to be shorter to keep the drone centered
-			_steps_counter = 5;
-			_max_steps = 10;
+			_sys_id.reset(state_init, _kInitVar);
 		}
 
 		break;
 
 	case state::pitch:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
-		    && ((now - _state_start_time) > 5_s)) {
+		if ((_sys_id.getVariances().max() < _kInitVar)
+		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(1);
 			_state = state::pitch_pause;
 			_state_start_time = now;
@@ -341,20 +338,14 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if ((now - _state_start_time) > 2_s) {
 			_state = state::yaw;
 			_state_start_time = now;
-			_sys_id.reset();
-			_input_scale = 1.f / (_param_mc_yawrate_p.get() * _param_mc_yawrate_k.get());
-			_signal_filter.reset(0.f);
-			_signal_sign = 1;
-			// first step needs to be shorter to keep the drone centered
-			_steps_counter = 5;
-			_max_steps = 10;
+			_sys_id.reset(state_init, _kInitVar);
 		}
 
 		break;
 
 	case state::yaw:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
-		    && ((now - _state_start_time) > 5_s)) {
+		if ((_sys_id.getVariances().max() < _kInitVar)
+		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(2);
 			_state = state::yaw_pause;
 			_state_start_time = now;
@@ -367,10 +358,6 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 			_state = state::verification;
 			_state_start_time = now;
 			_sys_id.reset();
-			_signal_filter.reset(0.f);
-			_signal_sign = 1;
-			_steps_counter = 5;
-			_max_steps = 10;
 		}
 
 		break;
@@ -445,7 +432,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 
 	if (_state != state::wait_for_disarm
 	    && _state != state::idle
-	    && (((now - _state_start_time) > 20_s)
+	    && (((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s + 2_s))
 		|| (fabsf(manual_control_setpoint.roll) > 0.05f)
 		|| (fabsf(manual_control_setpoint.pitch) > 0.05f))) {
 		_state = state::fail;
@@ -507,15 +494,6 @@ bool McAutotuneAttitudeControl::registerActuatorControlsCallback()
 	return true;
 }
 
-bool McAutotuneAttitudeControl::areAllSmallerThan(const Vector<float, 5> &vect, float threshold) const
-{
-	return (vect(0) < threshold)
-	       && (vect(1) < threshold)
-	       && (vect(2) < threshold)
-	       && (vect(3) < threshold)
-	       && (vect(4) < threshold);
-}
-
 void McAutotuneAttitudeControl::copyGains(int index)
 {
 	if (index <= 2) {
@@ -585,27 +563,27 @@ void McAutotuneAttitudeControl::stopAutotune()
 	_vehicle_torque_setpoint_sub.unregisterCallback();
 }
 
-const Vector3f McAutotuneAttitudeControl::getIdentificationSignal()
+const Vector3f McAutotuneAttitudeControl::getIdentificationSignal(hrt_abstime now)
 {
-	if (_steps_counter > _max_steps) {
-		_signal_sign = (_signal_sign == 1) ? 0 : 1;
-		_steps_counter = 0;
+	const float t = static_cast<float>(now - _state_start_time) * 1e-6f;
 
-		if (_max_steps > 1) {
-			_max_steps--;
+	float signal;
 
-		} else {
-			_max_steps = 5;
-		}
+	if (_param_mc_at_sysid_type.get() == static_cast<int32_t>(SignalType::kLinearSineSweep)) {
+		const float f_max = (_state == state::yaw) ? _param_mc_at_sysid_fyaw.get() : _param_mc_at_sysid_f1.get();
+		signal = signal_generator::getLinearSineSweep(_param_mc_at_sysid_f0.get(), f_max,
+				_param_mc_at_sysid_time.get(), t);
+
+	} else if (_param_mc_at_sysid_type.get() == static_cast<int32_t>(SignalType::kLogSineSweep)) {
+		const float f_max = (_state == state::yaw) ? _param_mc_at_sysid_fyaw.get() : _param_mc_at_sysid_f1.get();
+		signal = signal_generator::getLogSineSweep(_param_mc_at_sysid_f0.get(), f_max,
+				_param_mc_at_sysid_time.get(), t);
+
+	} else {
+		signal = 0.f;
 	}
 
-	_steps_counter++;
-
-	const float step = float(_signal_sign) * _param_mc_at_sysid_amp.get();
-
-	Vector3f rate_sp{};
-
-	const float signal = step - _signal_filter.getState();
+	Vector3f rate_sp;
 
 	if (_state == state::roll) {
 		rate_sp(0) = signal;
@@ -621,8 +599,6 @@ const Vector3f McAutotuneAttitudeControl::getIdentificationSignal()
 		rate_sp(1) = signal;
 	}
 
-	_signal_filter.update(step);
-
 	return rate_sp;
 }
 

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp

@@ -43,6 +43,7 @@
 #include <lib/perf/perf_counter.h>
 #include <lib/pid_design/pid_design.hpp>
 #include <lib/system_identification/system_identification.hpp>
+#include <lib/system_identification/signal_generator.hpp>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
@@ -93,14 +94,13 @@ private:
 	void updateStateMachine(hrt_abstime now);
 	bool registerActuatorControlsCallback();
 	void stopAutotune();
-	bool areAllSmallerThan(const matrix::Vector<float, 5> &vect, float threshold) const;
 	void copyGains(int index);
 	bool areGainsGood() const;
 	void saveGainsToParams();
 	void backupAndSaveGainsToParams();
 	void revertParamGains();
 
-	const matrix::Vector3f getIdentificationSignal();
+	const matrix::Vector3f getIdentificationSignal(hrt_abstime now);
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)};
 	uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)};
@@ -131,13 +131,17 @@ private:
 		wait_for_disarm = autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM
 	} _state{state::idle};
 
+	enum class SignalType : uint8_t {
+		kLinearSineSweep = 0,
+		kLogSineSweep
+	};
+
 	hrt_abstime _state_start_time{0};
-	uint8_t _steps_counter{0};
-	uint8_t _max_steps{5};
-	int8_t _signal_sign{0};
 
 	bool _armed{false};
 
+	matrix::Vector3f _angular_velocity{};
+
 	matrix::Vector3f _kid{};
 	matrix::Vector3f _rate_k{};
 	matrix::Vector3f _rate_i{};
@@ -150,27 +154,16 @@ private:
 
 	bool _gains_backup_available{false}; // true if a backup of the parameters has been done
 
-	/**
-	 * Scale factor applied to the input data to have the same input/output range
-	 * When input and output scales are a lot different, some elements of the covariance
-	 * matrix will collapse much faster than other ones, creating an ill-conditionned matrix
-	 */
-	float _input_scale{1.f};
-
 	hrt_abstime _last_run{0};
 	hrt_abstime _last_publish{0};
 	hrt_abstime _last_model_update{0};
 
 	float _interval_sum{0.f};
-	float _interval_count{0.f};
+	uint32_t _interval_count{0};
 	float _sample_interval_avg{0.01f};
 	float _filter_dt{0.01f};
 	bool _are_filters_initialized{false};
 
-	AlphaFilter<float> _signal_filter; ///< used to create a wash-out filter
-
-	static constexpr float _model_dt_min{2e-3f}; // 2ms = 500Hz
-	static constexpr float _model_dt_max{10e-3f}; // 10ms = 100Hz
 	int _model_update_scaler{1};
 	int _model_update_counter{0};
 
@@ -179,6 +172,11 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::MC_AT_START>) _param_mc_at_start,
 		(ParamFloat<px4::params::MC_AT_SYSID_AMP>) _param_mc_at_sysid_amp,
+		(ParamFloat<px4::params::MC_AT_SYSID_F0>) _param_mc_at_sysid_f0,
+		(ParamFloat<px4::params::MC_AT_SYSID_F1>) _param_mc_at_sysid_f1,
+		(ParamFloat<px4::params::MC_AT_SYSID_FYAW>) _param_mc_at_sysid_fyaw,
+		(ParamFloat<px4::params::MC_AT_SYSID_TIME>) _param_mc_at_sysid_time,
+		(ParamInt<px4::params::MC_AT_SYSID_TYPE>) _param_mc_at_sysid_type,
 		(ParamInt<px4::params::MC_AT_APPLY>) _param_mc_at_apply,
 		(ParamFloat<px4::params::MC_AT_RISE_TIME>) _param_mc_at_rise_time,
 
@@ -201,6 +199,6 @@ private:
 		(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p
 	)
 
-	static constexpr float _publishing_dt_s = 100e-3f;
-	static constexpr hrt_abstime _publishing_dt_hrt = 100_ms;
+	static constexpr float _publishing_dt_s = 5e-3f;
+	static constexpr float _kInitVar = 10.f;
 };

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c

@@ -76,6 +76,71 @@ PARAM_DEFINE_INT32(MC_AT_START, 0);
  */
 PARAM_DEFINE_FLOAT(MC_AT_SYSID_AMP, 0.7);
 
+/**
+ * Start frequency of the injected signal
+ *
+ * Can be set lower or higher than the end frequency
+ *
+ * @min 0.1
+ * @max 30.0
+ * @decimal 1
+ * @unit Hz
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(MC_AT_SYSID_F0, 1.f);
+
+/**
+ * End frequency of the injected signal
+ *
+ * Can be set lower or higher than the start frequency
+ *
+ * @min 0.1
+ * @max 30.0
+ * @decimal 1
+ * @unit Hz
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(MC_AT_SYSID_F1, 20.f);
+
+/**
+ * Yaw axis maximum frequency
+ *
+ * End frequency of the identification for the yaw axis.
+ * This is usually set lower than the roll and pitch axes
+ * to only consider the yaw actuation produced by propeller drag.
+ *
+ * @min 0.1
+ * @max 30.0
+ * @decimal 1
+ * @unit Hz
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(MC_AT_SYSID_FYAW, 5.f);
+
+/**
+ * Maneuver time for each axis
+ *
+ * Duration of the input signal sent on each axis during system identification
+ *
+ * @min 5
+ * @max 120
+ * @decimal 0
+ * @unit s
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(MC_AT_SYSID_TIME, 10.f);
+
+/**
+ * Input signal type
+ *
+ * Type of signal used during system identification to excite the system.
+ *
+ * @value 0 Linear sine sweep
+ * @value 1 Logarithmic sine sweep
+ * @group Autotune
+ */
+PARAM_DEFINE_INT32(MC_AT_SYSID_TYPE, 1);
+
 /**
  * Controls when to apply the new gains
  *