Cleaned up the API and added SBUS support

With this margin it is made sure that if the loiter is not perfectly tracked,
(vehicle outside of path setpoint) a wp just at the border of the loiter
is still reachable.
It should though be as small as necessary, as otherwise, with good
loiter tracking,  waypoints that are close but not right on the loiter
radius are not enforcing the exit course neither.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover

@@ -4,31 +4,9 @@
 # @type Rover
 # @class Rover
 
-. ${R}etc/init.d/rc.rover_defaults
+. ${R}etc/init.d/rc.rover_differential_defaults
 
-param set-default GND_L1_DIST 5
-param set-default GND_SP_CTRL_MODE 1
-param set-default GND_SPEED_D 3
-param set-default GND_SPEED_I 0.001
-param set-default GND_SPEED_IMAX 0.125
-param set-default GND_SPEED_P 0.25
-param set-default GND_SPEED_THR_SC 1
-param set-default GND_SPEED_TRIM 4
-param set-default GND_THR_CRUISE 0.3
-param set-default GND_THR_MAX 0.5
-param set-default GND_THR_MIN 0
-
-param set-default NAV_ACC_RAD 0.5
-param set-default NAV_LOITER_RAD 2
-
-param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
-
-param set-default CA_AIRFRAME 6
-
-param set-default CA_R_REV 3
 param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 101
 param set-default PWM_MAIN_FUNC6 102
 param set-default PWM_MAIN_FUNC7 102
-

ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol

@@ -66,7 +66,6 @@ param set-default SIM_GZ_SV_FUNC2 202
 param set-default SIM_GZ_SV_FUNC3 203
 
 param set-default COM_RC_IN_MODE 1
-param set-default ASPD_PRIMARY 1
 
 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -0,0 +1,31 @@
+#!/bin/sh
+# @name Aion Robotics R1 Rover
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_differential_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
+param set-default SIM_GZ_WH_MIN2 0
+param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_DIS2 100
+
+param set-default SIM_GZ_WH_REV 1 # reverse right wheel

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -80,6 +80,7 @@ px4_add_romfs_files(
 	4005_gz_x500_vision
 	4006_gz_px4vision
 	4008_gz_advanced_plane
+	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
 
 	6011_gazebo-classic_typhoon_h480

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -153,7 +153,6 @@ fi
 
 param set-default BAT1_N_CELLS 4
 
-param set-default CBRK_AIRSPD_CHK 0
 param set-default CBRK_SUPPLY_CHK 894281
 
 # disable check, no CPU load reported on posix yet

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -51,6 +51,8 @@ px4_add_romfs_files(
 	rc.thermal_cal
 	rc.rover_apps
 	rc.rover_defaults
+	rc.rover_differential_apps
+	rc.rover_differential_defaults
 	rc.uuv_apps
 	rc.uuv_defaults
 	rc.vehicle_setup

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -28,13 +28,13 @@ param set-default BAT1_CAPACITY 23000
 param set-default BAT1_N_CELLS 4
 param set-default BAT1_R_INTERNAL 0.0025
 
-param set-default CBRK_AIRSPD_CHK 162128
+param set-default SYS_HAS_NUM_ASPD 0
 param set-default CBRK_IO_SAFETY 22027
 
 param set-default EKF2_GPS_POS_X -0.12
 param set-default EKF2_IMU_POS_X -0.12
 
-param set-default FW_ARSP_MODE 1
+param set-default FW_USE_AIRSPD 0
 param set-default NPFG_PERIOD 25
 param set-default FW_PR_FF 0.7
 param set-default FW_PR_I 0.18

ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover

@@ -11,52 +11,15 @@
 # @board bitcraze_crazyflie exclude
 #
 
-. ${R}etc/init.d/rc.rover_defaults
-
-
+. ${R}etc/init.d/rc.rover_differential_defaults
 
 param set-default BAT1_N_CELLS 4
 
 param set-default EKF2_GBIAS_INIT 0.01
 param set-default EKF2_ANGERR_INIT 0.01
-param set-default EKF2_MAG_TYPE 1
-
-param set-default FW_AIRSPD_MIN 0
-param set-default FW_AIRSPD_TRIM 1
-param set-default FW_AIRSPD_MAX 3
-
-param set-default GND_SP_CTRL_MODE 1
-param set-default GND_L1_DIST 5
-param set-default GND_L1_PERIOD 3
-param set-default GND_THR_CRUISE 0.7
-param set-default GND_THR_MAX 0.5
-
-# Because this is differential drive, it can make a turn with radius 0.
-# This corresponds to a turn angle of pi radians.
-# If a special case is made for differential-drive, this will need to change.
-param set-default GND_MAX_ANG 3.142
-param set-default GND_WHEEL_BASE 0.3
-
-# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
-# to support negative throttle.
-param set-default GND_THR_MIN 0
-param set-default GND_SPEED_P 0.25
-param set-default GND_SPEED_I 3
-param set-default GND_SPEED_D 0.001
-param set-default GND_SPEED_IMAX 0.125
-param set-default GND_SPEED_THR_SC 1
-
-param set-default NAV_ACC_RAD 0.5
-
-# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
-param set-default GND_MAX_ANG 3.1415
 
 # Set geometry & output configration
-param set-default CA_AIRFRAME 6
-param set-default CA_R_REV 3
-
-
 param set-default RBCLW_ADDRESS 128
 param set-default RBCLW_FUNC1 101
 param set-default RBCLW_FUNC2 102
-param set-default RBCLW_REV 1
+param set-default RBCLW_REV 1 # reverse right wheels

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -54,3 +54,5 @@ param set-default MIS_TKO_LAND_REQ 2
 # FW takeoff acceleration can easily exceed ublox GPS 2G default.
 #
 param set-default GPS_UBX_DYNMODEL 8
+
+param set-default SYS_HAS_NUM_ASPD 1 # by default require an airspeed sensor

ROMFS/px4fmu_common/init.d/rc.rover_apps

@@ -9,8 +9,6 @@
 # Start the attitude and position estimator.
 #
 ekf2 start &
-#attitude_estimator_q start
-#local_position_estimator start
 
 #
 # Start Control Allocator
@@ -22,7 +20,6 @@ control_allocator start
 #
 rover_pos_control start
 
-
 #
 # Start Land Detector.
 #

ROMFS/px4fmu_common/init.d/rc.rover_defaults

@@ -10,9 +10,6 @@ set VEHICLE_TYPE rover
 # MAV_TYPE_GROUND_ROVER 10
 param set-default MAV_TYPE 10
 
-# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
-param set-default CBRK_AIRSPD_CHK 162128
-
 param set-default MIS_TAKEOFF_ALT 0.01
 
 param set-default NAV_ACC_RAD 2

ROMFS/px4fmu_common/init.d/rc.rover_differential_apps

@@ -0,0 +1,11 @@
+#!/bin/sh
+# Standard apps for a differential drive rover.
+
+# Start the attitude and position estimator.
+ekf2 start &
+
+# Start rover differential drive controller.
+differential_drive_control start
+
+# Start Land Detector.
+land_detector start rover

ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults

@@ -0,0 +1,11 @@
+#!/bin/sh
+# Differential rover parameters.
+
+set VEHICLE_TYPE rover_differential
+
+param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
+
+param set-default CA_AIRFRAME 6 # Rover (Differential)
+param set-default CA_R_REV 3 # Right and left motors reversible
+
+param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying

ROMFS/px4fmu_common/init.d/rc.uuv_defaults

@@ -9,6 +9,3 @@ set VEHICLE_TYPE uuv
 
 # MAV_TYPE_SUBMARINE 12
 param set-default MAV_TYPE 12
-
-# UUV don't have an airspeed sensor, so disable checks around it
-param set-default CBRK_AIRSPD_CHK 162128

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -32,6 +32,15 @@ then
 	. ${R}etc/init.d/rc.rover_apps
 fi
 
+#
+# Differential Rover setup.
+#
+if [ $VEHICLE_TYPE = rover_differential ]
+then
+	# Start differential drive rover apps.
+	. ${R}etc/init.d/rc.rover_differential_apps
+fi
+
 #
 # VTOL setup.
 #

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -42,4 +42,6 @@ param set-default MPC_MAN_Y_MAX 90
 
 param set-default RTL_TYPE 1
 
+param set-default SYS_HAS_NUM_ASPD 1 # by default require an airspeed sensor
+
 param set-default WV_EN 1

boards/modalai/voxl2/default.px4board

@@ -3,6 +3,7 @@ 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-v5x/default.px4board

@@ -53,6 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y

boards/px4/fmu-v6xrt/init/rc.board_sensors

@@ -61,8 +61,8 @@ then
 	fi
 fi
 
-# Internal SPI bus ICM42688p (hard-mounted)
-icm42688p -R 12 -b 1 -s start
+# Internal SPI bus ICM42686p (hard-mounted)
+icm42688p -6 -R 12 -b 1 -s start
 
 # Internal on IMU SPI BMI088
 bmi088 -A -R 4 -s start

boards/px4/fmu-v6xrt/src/spi.cpp

@@ -65,7 +65,7 @@
 
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::LPSPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */
 	}, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33  GPIO2_IO01
 
 	initSPIBus(SPI::Bus::LPSPI2, {

boards/px4/sitl/default.px4board

@@ -12,6 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_EVENTS=y

msg/CMakeLists.txt

@@ -69,6 +69,7 @@ set(msg_files
 	DebugKeyValue.msg
 	DebugValue.msg
 	DebugVect.msg
+	DifferentialDriveSetpoint.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
 	Ekf2Timestamps.msg

msg/DifferentialDriveSetpoint.msg

@@ -0,0 +1,4 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 speed # [m/s] collective roll-off speed in body x-axis
+float32 yaw_rate # [rad/s] yaw rate

msg/PositionSetpoint.msg

@@ -22,11 +22,7 @@ float32 vz			# local velocity setpoint in m/s in NED
 float64 lat			# latitude, in deg
 float64 lon			# longitude, in deg
 float32 alt			# altitude AMSL, in m
-float32 yaw			# yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
-bool yaw_valid			# true if yaw setpoint valid
-
-float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
-bool yawspeed_valid		# true if yawspeed setpoint valid
+float32 yaw			# yaw (only in hover), in rad [-PI..PI), NaN = leave to flight task
 
 float32 loiter_radius		# loiter major axis radius in m
 float32 loiter_minor_radius	# loiter minor axis radius (used for non-circular loiter shapes) in m
@@ -39,5 +35,3 @@ float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoi
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
-
-bool disable_weather_vane   # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind

platforms/common/include/px4_platform_common/module.h

@@ -91,7 +91,7 @@ extern pthread_mutex_t px4_modules_mutex;
  *      static int custom_command(int argc, char *argv[])
  *      {
  *              // support for custom commands
- *              // it none are supported, just do:
+ *              // if none are supported, just do:
  *              return print_usage("unrecognized command");
  *      }
  *

platforms/ros2/include/px4_platform_common/module.h

@@ -75,7 +75,7 @@
  *      static int custom_command(int argc, char *argv[])
  *      {
  *              // support for custom commands
- *              // it none are supported, just do:
+ *              // if none are supported, just do:
  *              return print_usage("unrecognized command");
  *      }
  *

src/drivers/gps/CMakeLists.txt

@@ -56,4 +56,5 @@ 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,6 +81,7 @@
 #include <linux/spi/spidev.h>
 #endif /* __PX4_LINUX */
 
+using namespace device;
 using namespace time_literals;
 
 #define TIMEOUT_1HZ		1300	//!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
@@ -169,7 +170,10 @@ public:
 	void reset_if_scheduled();
 
 private:
-	int				_serial_fd{-1};					///< serial interface to GPS
+#ifdef __PX4_LINUX
+	int				_spi_fd {-1};					///< SPI interface to GPS
+#endif
+	Serial 			*_uart = nullptr;	
 	unsigned			_baudrate{0};					///< current baudrate
 	const unsigned			_configured_baudrate{0};			///< configured baudrate (0=auto-detect)
 	char				_port[20] {};					///< device / serial port path
@@ -329,8 +333,10 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
 		set_device_bus(c - 48); // sub 48 to convert char to integer
 
+#ifdef __PX4_LINUX
 	} else if (_interface == GPSHelper::Interface::SPI) {
 		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
+#endif
 	}
 
 	if (_mode == gps_driver_mode_t::None) {
@@ -403,10 +409,22 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 			return num_read;
 		}
 
-	case GPSCallbackType::writeDeviceData:
-		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
+	case GPSCallbackType::writeDeviceData: {
+			gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
 
-		return ::write(gps->_serial_fd, data1, (size_t)data2);
+			int ret = 0;
+
+			if (gps->_uart) {
+				ret = gps->_uart->write((void *) data1, (size_t) data2);
+
+#ifdef __PX4_LINUX
+			} else if (gps->_spi_fd >= 0) {
+				ret = ::write(gps->_spi_fd, data1, (size_t)data2);
+#endif
+			}
+
+			return ret;
+		}
 
 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -437,10 +455,11 @@ 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;
 	}
 
@@ -449,72 +468,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 
 int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 {
+	int ret = 0;
+	const unsigned character_count = 32; // minimum bytes that we want to read
+	const int max_timeout = 50;
+	int timeout_adjusted = math::min(max_timeout, timeout);
+
 	handleInjectDataTopic();
 
-#if !defined(__PX4_QURT)
+	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
+		ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
 
-	/* For non QURT, use the usual polling. */
+// SPI is only supported on Linux
+#if defined(__PX4_LINUX)
 
-	//Poll only for the serial data. In the same thread we also need to handle orb messages,
-	//so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
-	//two pollings use different underlying mechanisms (at least under posix), which makes this
-	//impossible. Instead we limit the maximum polling interval and regularly check for new orb
-	//messages.
-	//FIXME: add a unified poll() API
-	const int max_timeout = 50;
+	} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
 
-	pollfd fds[1];
-	fds[0].fd = _serial_fd;
-	fds[0].events = POLLIN;
+		//Poll only for the SPI data. In the same thread we also need to handle orb messages,
+		//so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the
+		//two pollings use different underlying mechanisms (at least under posix), which makes this
+		//impossible. Instead we limit the maximum polling interval and regularly check for new orb
+		//messages.
+		//FIXME: add a unified poll() API
 
-	int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
+		pollfd fds[1];
+		fds[0].fd = _spi_fd;
+		fds[0].events = POLLIN;
 
-	if (ret > 0) {
-		/* if we have new data from GPS, go handle it */
-		if (fds[0].revents & POLLIN) {
-			/*
-			 * We are here because poll says there is some data, so this
-			 * won't block even on a blocking device. But don't read immediately
-			 * by 1-2 bytes, wait for some more data to save expensive read() calls.
-			 * If we have all requested data available, read it without waiting.
-			 * If more bytes are available, we'll go back to poll() again.
-			 */
-			const unsigned character_count = 32; // minimum bytes that we want to read
-			unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
-			const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
+		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
 
-#ifdef __PX4_NUTTX
-			int err = 0;
-			int bytes_available = 0;
-			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
+		if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device. But don't read immediately
+				 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+				 * If we have all requested data available, read it without waiting.
+				 * If more bytes are available, we'll go back to poll() again.
+				 */
+				unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
+				const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
 
-			if (err != 0 || bytes_available < (int)character_count) {
 				px4_usleep(sleeptime);
+
+				ret = ::read(_spi_fd, buf, buf_length);
+
+				if (ret > 0) {
+					_num_bytes_read += ret;
+				}
+
+			} else {
+				ret = -1;
 			}
-
-#else
-			px4_usleep(sleeptime);
-#endif
-
-			ret = ::read(_serial_fd, buf, buf_length);
-
-			if (ret > 0) {
-				_num_bytes_read += ret;
-			}
-
-		} else {
-			ret = -1;
 		}
+
+#endif
 	}
 
 	return ret;
-
-#else
-	/* For QURT, just use read for now, since this doesn't block, we need to slow it down
-	 * just a bit. */
-	px4_usleep(10000);
-	return ::read(_serial_fd, buf, buf_length);
-#endif
 }
 
 void GPS::handleInjectDataTopic()
@@ -583,105 +594,38 @@ bool GPS::injectData(uint8_t *data, size_t len)
 {
 	dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
 
-	size_t written = ::write(_serial_fd, data, len);
-	::fsync(_serial_fd);
+	size_t written = 0;
+
+	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
+		written = _uart->write((const void *) data, len);
+
+#ifdef __PX4_LINUX
+
+	} else if (_interface == GPSHelper::Interface::SPI) {
+		written = ::write(_spi_fd, data, len);
+		::fsync(_spi_fd);
+#endif
+	}
+
 	return written == len;
 }
 
 int GPS::setBaudrate(unsigned baud)
 {
-	/* process baud rate */
-	int speed;
+	if (_interface == GPSHelper::Interface::UART) {
+		if ((_uart) && (_uart->setBaudrate(baud))) {
+			return 0;
+		}
 
-	switch (baud) {
-	case 9600:   speed = B9600;   break;
+#ifdef __PX4_LINUX
 
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-#ifndef B460800
-#define B460800 460800
+	} else if (_interface == GPSHelper::Interface::SPI) {
+		// Can't set the baudrate on a SPI port but just return a success
+		return 0;
 #endif
-
-	case 460800: speed = B460800; break;
-
-#ifndef B921600
-#define B921600 921600
-#endif
-
-	case 921600: speed = B921600; break;
-
-	default:
-		PX4_ERR("ERR: unknown baudrate: %d", baud);
-		return -EINVAL;
 	}
 
-	struct termios uart_config;
-
-	int termios_state;
-
-	/* fill the struct for the new configuration */
-	tcgetattr(_serial_fd, &uart_config);
-
-	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
-
-	//
-	// Input flags - Turn off input processing
-	//
-	// convert break to null byte, no CR to NL translation,
-	// no NL to CR translation, don't mark parity errors or breaks
-	// no input parity check, don't strip high bit off,
-	// no XON/XOFF software flow control
-	//
-	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
-				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
-	//
-	// Output flags - Turn off output processing
-	//
-	// no CR to NL translation, no NL to CR-NL translation,
-	// no NL to CR translation, no column 0 CR suppression,
-	// no Ctrl-D suppression, no fill characters, no case mapping,
-	// no local output processing
-	//
-	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
-	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
-	uart_config.c_oflag = 0;
-
-	//
-	// No line processing
-	//
-	// echo off, echo newline off, canonical mode off,
-	// extended input processing off, signal chars off
-	//
-	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-
-	/* no parity, one stop bit, disable flow control */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
-
-	/* set baud rate */
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
-		return -1;
-	}
-
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
-		return -1;
-	}
-
-	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
-		GPS_ERR("ERR: %d (tcsetattr)", termios_state);
-		return -1;
-	}
-
-	return 0;
+	return -1;
 }
 
 void GPS::initializeCommunicationDump()
@@ -840,31 +784,58 @@ GPS::run()
 			_helper = nullptr;
 		}
 
-		if (_serial_fd < 0) {
-			/* open the serial port */
-			_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+		if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
 
-			if (_serial_fd < 0) {
-				PX4_ERR("failed to open %s err: %d", _port, errno);
-				px4_sleep(1);
+			// 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);
 				continue;
 			}
 
 #ifdef __PX4_LINUX
 
-			if (_interface == GPSHelper::Interface::SPI) {
-				int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
-				int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
+			_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
 
-				if (status_value < 0) {
-					PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-				}
+			if (_spi_fd < 0) {
+				PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
+				px4_sleep(1);
+				continue;
+			}
 
-				status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
+			int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
+			int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
 
-				if (status_value < 0) {
-					PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-				}
+			if (status_value < 0) {
+				PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+			}
+
+			status_value = ::ioctl(_spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
+
+			if (status_value < 0) {
+				PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
 			}
 
 #endif /* __PX4_LINUX */
@@ -1056,9 +1027,17 @@ GPS::run()
 			}
 		}
 
-		if (_serial_fd >= 0) {
-			::close(_serial_fd);
-			_serial_fd = -1;
+		if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
+			(void) _uart->close();
+			delete _uart;
+			_uart = nullptr;
+
+#ifdef __PX4_LINUX
+
+		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
+			::close(_spi_fd);
+			_spi_fd = -1;
+#endif
 		}
 
 		if (_mode_auto) {
@@ -1477,12 +1456,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'i':
-			if (!strcmp(myoptarg, "spi")) {
-				interface = GPSHelper::Interface::SPI;
-
-			} else if (!strcmp(myoptarg, "uart")) {
+			if (!strcmp(myoptarg, "uart")) {
 				interface = GPSHelper::Interface::UART;
-
+#ifdef __PX4_LINUX
+			} else if (!strcmp(myoptarg, "spi")) {
+				interface = GPSHelper::Interface::SPI;
+#endif
 			} else {
 				PX4_ERR("unknown interface: %s", myoptarg);
 				error_flag = true;
@@ -1490,12 +1469,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'j':
-			if (!strcmp(myoptarg, "spi")) {
-				interface_secondary = GPSHelper::Interface::SPI;
-
-			} else if (!strcmp(myoptarg, "uart")) {
+			if (!strcmp(myoptarg, "uart")) {
 				interface_secondary = GPSHelper::Interface::UART;
-
+#ifdef __PX4_LINUX
+			} else if (!strcmp(myoptarg, "spi")) {
+				interface_secondary = GPSHelper::Interface::SPI;
+#endif
 			} else {
 				PX4_ERR("unknown interface for secondary: %s", myoptarg);
 				error_flag = true;

src/drivers/imu/invensense/icm42688p/ICM42688P.cpp

@@ -126,8 +126,9 @@ int ICM42688P::probe()
 {
 	for (int i = 0; i < 3; i++) {
 		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+		uint8_t expected_whoami = isICM686 ? WHOAMI686 : WHOAMI;
 
-		if (whoami == WHOAMI || (isICM686 && whoami == WHOAMI686)) {
+		if (whoami == expected_whoami) {
 			return PX4_OK;
 
 		} else {

src/lib/circuit_breaker/circuit_breaker.h

@@ -53,7 +53,6 @@
 #define CBRK_BUZZER_KEY         782097
 #define CBRK_SUPPLY_CHK_KEY	894281
 #define CBRK_IO_SAFETY_KEY	22027
-#define CBRK_AIRSPD_CHK_KEY	162128
 #define CBRK_FLIGHTTERM_KEY	121212
 #define CBRK_USB_CHK_KEY	197848
 #define CBRK_VTOLARMING_KEY	159753

src/lib/circuit_breaker/circuit_breaker_params.c

@@ -69,21 +69,6 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
  */
 PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 22027);
 
-/**
- * Circuit breaker for airspeed sensor
- *
- * Setting this parameter to 162128 will disable the check for an airspeed sensor.
- * The sensor driver will not be started and it cannot be calibrated.
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @reboot_required true
- * @min 0
- * @max 162128
- * @category Developer
- * @group Circuit Breaker
- */
-PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
-
 /**
  * Circuit breaker for flight termination
  *

src/lib/drivers/device/CMakeLists.txt

@@ -40,20 +40,31 @@ 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
-	list(APPEND SRCS_PLATFORM
-		posix/I2C.cpp
-		posix/SPI.cpp
-	)
+	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)
 endif()
 
 px4_add_library(drivers__device
 	CDev.cpp
+	Serial.cpp
 	${SRCS_PLATFORM}
 	)
 

src/lib/drivers/device/Serial.cpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#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

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   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

@@ -0,0 +1,363 @@
+/****************************************************************************
+ *
+ *   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/nuttx/SerialImpl.hpp

@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   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);
+
+	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

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

@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   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

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   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,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 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,29 +31,50 @@
  *
  ****************************************************************************/
 
-#include "ActuatorEffectivenessRoverDifferential.hpp"
-#include <ControlAllocation/ControlAllocation.hpp>
+#include "SerialSBUSImpl.hpp"
 
-using namespace matrix;
+#include <sys/ioctl.h>
+#include <asm-generic/termbits.h>
+#include <px4_log.h>
 
-bool
-ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration,
-		EffectivenessUpdateReason external_update)
+namespace device
 {
-	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+
+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;
 	}
 
-	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f});
-	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f});
-	_motors_mask = (1u << 0) | (1u << 1);
 	return true;
 }
 
-void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
-{
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
-}
-
+} // namespace device

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

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   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 SerialSBUSImpl
+{
+public:
+
+	SerialSBUSImpl() {};
+	virtual ~SerialSBUSImpl() {};
+
+	bool configure(int fd, uint32_t baud);
+
+	static const uint32_t DEFAULT_BAUDRATE{100000};
+};
+
+} // namespace device

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

@@ -0,0 +1,159 @@
+/****************************************************************************
+ *
+ *   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/posix/SerialStandardImpl.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 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
@@ -33,21 +33,23 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include <stdint.h>
 
-class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness
+namespace device
+{
+
+class SerialStandardImpl
 {
 public:
-	ActuatorEffectivenessRoverDifferential() = default;
-	virtual ~ActuatorEffectivenessRoverDifferential() = default;
 
-	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+	SerialStandardImpl() {};
+	virtual ~SerialStandardImpl() {};
 
-	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+	bool configure(int fd, uint32_t baud);
 
-	const char *name() const override { return "Rover (Differential)"; }
 private:
-	uint32_t _motors_mask{};
+	bool validateBaudrate(uint32_t baudrate);
+
 };
+
+} // namespace device

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

@@ -0,0 +1,239 @@
+/****************************************************************************
+ *
+ *   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

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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/matrix/matrix/Slice.hpp

@@ -35,6 +35,8 @@ public:
 		assert(y0 + Q <= N);
 	}
 
+	Slice(const Slice<Type, P, Q, M, N> &other) = default;
+
 	const Type &operator()(size_t i, size_t j) const
 	{
 		assert(i < P);
@@ -52,6 +54,12 @@ public:
 		return (*_data)(_x0 + i, _y0 + j);
 	}
 
+	// Separate function needed otherwise the default copy constructor matches before the deep copy implementation
+	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, M, N> &other)
+	{
+		return this->operator=<M, N>(other);
+	}
+
 	template<size_t MM, size_t NN>
 	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
 	{

src/lib/matrix/test/MatrixSliceTest.cpp

@@ -262,3 +262,12 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
+
+TEST(MatrixSliceTest, XYAssignmentTest)
+{
+	Vector3f a(1, 2, 3);
+	Vector3f b(4, 5, 6);
+	// Assign first two elements from b to first two slot of a
+	a.xy() = b.xy();
+	EXPECT_EQ(a, Vector3f(4, 5, 3));
+}

src/lib/npfg/npfg.hpp

@@ -85,7 +85,7 @@ public:
 
 	float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const;
 	/*
-	 * Computes the lateral acceleration and airspeed references necessary to track
+	 * Computes the lateral acceleration and true airspeed references necessary to track
 	 * a path in wind (including excess wind conditions).
 	 *
 	 * @param[in] curr_pos_local Current horizontal vehicle position in local coordinates [m]
@@ -150,12 +150,12 @@ public:
 	void setMaxTrackKeepingMinGroundSpeed(float min_gsp) { min_gsp_track_keeping_max_ = math::max(min_gsp, 0.0f); }
 
 	/*
-	 * Set the nominal airspeed reference [m/s].
+	 * Set the nominal airspeed reference (true airspeed) [m/s].
 	 */
 	void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); }
 
 	/*
-	 * Set the maximum airspeed reference [m/s].
+	 * Set the maximum airspeed reference (true airspeed) [m/s].
 	 */
 	void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, 0.1f); }
 
@@ -306,8 +306,8 @@ private:
 	// ^disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference
 
 	// guidance settings
-	float airspeed_nom_{15.0f}; // nominal (desired) airspeed reference (generally equivalent to cruise optimized airspeed) [m/s]
-	float airspeed_max_{20.0f}; // maximum airspeed reference - the maximum achievable/allowed airspeed reference [m/s]
+	float airspeed_nom_{15.0f}; // nominal (desired) true airspeed reference (generally equivalent to cruise optimized airspeed) [m/s]
+	float airspeed_max_{20.0f}; // maximum true airspeed reference - the maximum achievable/allowed airspeed reference [m/s]
 	float roll_time_const_{0.0f}; // autopilot roll response time constant [s]
 	float min_gsp_desired_{0.0f}; // user defined miminum desired forward ground speed [m/s]
 	float min_gsp_track_keeping_max_{5.0f}; // maximum, minimum forward ground speed demand from track keeping logic [m/s]
@@ -341,8 +341,8 @@ private:
 	 * guidance outputs
 	 */
 
-	float airspeed_ref_{15.0f}; // airspeed reference [m/s]
-	matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // air velocity reference vector [m/s]
+	float airspeed_ref_{15.0f}; // true airspeed reference [m/s]
+	matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // true air velocity reference vector [m/s]
 	float lateral_accel_{0.0f}; // lateral acceleration reference [m/s^2]
 	float lateral_accel_ff_{0.0f}; // lateral acceleration demand to maintain path curvature [m/s^2]
 
@@ -358,7 +358,7 @@ private:
 	 * condition, path properties, and stability bounds.
 	 *
 	 * @param[in] ground_speed Vehicle ground speed [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @param[in] track_error Track error (magnitude) [m]
 	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
@@ -384,7 +384,7 @@ private:
 	/*
 	 * Cacluates an approximation of the wind factor (see [TODO: include citation]).
 	 *
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @return Non-dimensional wind factor approximation
 	 */
@@ -395,7 +395,7 @@ private:
 	 * track keeping stability.
 	 *
 	 * @param[in] air_turn_rate The turn rate required to track the current path
-	 *            curvature at the current airspeed, in a no-wind condition [rad/s]
+	 *            curvature at the current true airspeed, in a no-wind condition [rad/s]
 	 * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation])
 	 * @return Period upper bound [s]
 	 */
@@ -408,7 +408,7 @@ private:
 	 * and a safety factor should be applied in addition to the returned value.
 	 *
 	 * @param[in] air_turn_rate The turn rate required to track the current path
-	 *            curvature at the current airspeed, in a no-wind condition [rad/s]
+	 *            curvature at the current true airspeed, in a no-wind condition [rad/s]
 	 * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation])
 	 * @return Period lower bound [s]
 	 */
@@ -493,8 +493,8 @@ private:
 
 	/*
 	 * Determines a reference air velocity *without curvature compensation, but
-	 * including "optimal" airspeed reference compensation in excess wind conditions.
-	 * Nominal and maximum airspeed member variables must be set before using this method.
+	 * including "optimal" true airspeed reference compensation in excess wind conditions.
+	 * Nominal and maximum true airspeed member variables must be set before using this method.
 	 *
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 * @param[in] bearing_vec Bearing vector
@@ -512,7 +512,7 @@ private:
 	 * Projection of the air velocity vector onto the bearing line considering
 	 * a connected wind triangle.
 	 *
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
 	 * @return Projection of air velocity vector on bearing vector [m/s]
 	 */
@@ -523,7 +523,7 @@ private:
 	 *
 	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
 	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible
 	 */
@@ -549,7 +549,7 @@ private:
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 * @param[in] bearing_vec Bearing vector
 	 * @param[in] wind_speed Wind speed [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @return Air velocity vector [m/s]
 	 */
 	matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
@@ -562,7 +562,7 @@ private:
 	 *
 	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
 	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @return bearing feasibility
 	 */
@@ -577,7 +577,7 @@ private:
 	 *            in direction of path
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @param[in] signed_track_error Signed error to track at closest point (sign
 	 *             determined by path normal direction) [m]
@@ -594,7 +594,7 @@ private:
 	 *
 	 * @param[in] air_vel Vechile air velocity vector [m/s]
 	 * @param[in] air_vel_ref Reference air velocity vector [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @return Lateral acceleration demand [m/s^2]
 	 */
 	float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,

src/lib/parameters/param_translation.cpp

@@ -42,7 +42,36 @@
 
 bool param_modify_on_import(bson_node_t node)
 {
+	// 2023-12-06: translate and invert FW_ARSP_MODE-> FW_USE_AIRSPD
+	{
+		if (strcmp("FW_ARSP_MODE", node->name) == 0) {
+			if (node->i32 == 0) {
+				node->i32 = 1;
 
+			} else {
+				node->i32 = 0;
+			}
+
+			strcpy(node->name, "FW_USE_AIRSPD");
+			PX4_INFO("copying and inverting %s -> %s", "FW_ARSP_MODE", "FW_USE_AIRSPD");
+			return true;
+		}
+	}
+
+	// 2023-12-06: translate CBRK_AIRSPD_CHK-> SYS_HAS_NUM_ASPD
+	{
+		if (strcmp("CBRK_AIRSPD_CHK", node->name) == 0) {
+			if (node->i32 == 162128) {
+				node->i32 = 0;
+
+				strcpy(node->name, "SYS_HAS_NUM_ASPD");
+				PX4_INFO("copying %s -> %s", "CBRK_AIRSPD_CHK", "SYS_HAS_NUM_ASPD");
+
+			}
+
+			return true;
+		}
+	}
 
 	return false;
 }

src/lib/systemlib/system_params.c

@@ -216,6 +216,19 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1);
  */
 PARAM_DEFINE_INT32(SYS_HAS_BARO, 1);
 
+/**
+ * Control if the vehicle has an airspeed sensor
+ *
+ * Set this to 0 if the board has no airspeed sensor.
+ * If set to 0, the preflight checks will not check for the presence of an
+ * airspeed sensor.
+ *
+ * @group System
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(SYS_HAS_NUM_ASPD, 0);
+
 /**
  * Number of distance sensors to check being available
  *

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -600,9 +600,8 @@ void AirspeedModule::select_airspeed_and_publish()
 	}
 
 	// check if airspeed based on ground-wind speed is valid and can be published
-	if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
-	    (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
-	     || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
+	if (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
+	    || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX) {
 
 		// _vehicle_local_position_valid determines if ground-wind estimate is valid
 		if (_vehicle_local_position_valid &&

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -132,7 +132,6 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_3, 1.0f);
 /**
  * Index or primary airspeed measurement source
  *
- * @value -1 Disabled
  * @value 0 Groundspeed minus windspeed
  * @value 1 First airspeed sensor
  * @value 2 Second airspeed sensor

src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-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
@@ -37,24 +37,20 @@
 using namespace time_literals;
 
 AirspeedChecks::AirspeedChecks()
-	: _param_fw_arsp_mode_handle(param_find("FW_ARSP_MODE")), _param_fw_airspd_max_handle(param_find("FW_AIRSPD_MAX"))
+	: _param_fw_airspd_max_handle(param_find("FW_AIRSPD_MAX"))
 {
 }
 
 void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(), CBRK_AIRSPD_CHK_KEY) ||
+	if (_param_sys_has_num_aspd.get() <= 0 ||
 	    (context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !context.status().is_vtol)) {
 		return;
 	}
 
-	int32_t airspeed_mode = 0;
-	param_get(_param_fw_arsp_mode_handle, &airspeed_mode);
-	const bool optional = (airspeed_mode == 1);
-
 	airspeed_validated_s airspeed_validated;
 
-	if (_airspeed_validated_sub.copy(&airspeed_validated) && hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s) {
+	if (_airspeed_validated_sub.copy(&airspeed_validated) && hrt_elapsed_time(&airspeed_validated.timestamp) < 2_s) {
 
 		reporter.setIsPresent(health_component_t::differential_pressure);
 
@@ -100,13 +96,13 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
 			}
 		}
 
-	} else if (!optional) {
+	} else {
 
 		/* EVENT
 		 * @description
 		 * <profile name="dev">
 		 * Most likely the airspeed selector module is not running.
-		 * This check can be configured via <param>CBRK_AIRSPD_CHK</param> parameter.
+		 * This check can be configured via <param>SYS_HAS_NUM_ASPD</param> parameter.
 		 * </profile>
 		 */
 		reporter.healthFailure(NavModes::All, health_component_t::differential_pressure,

src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-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
@@ -49,10 +49,9 @@ public:
 private:
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 
-	const param_t _param_fw_arsp_mode_handle;
 	const param_t _param_fw_airspd_max_handle;
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk
+					(ParamInt<px4::params::SYS_HAS_NUM_ASPD>) _param_sys_has_num_aspd
 				       )
 };

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -62,8 +62,6 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessTailsitterVTOL.hpp
 	ActuatorEffectivenessRoverAckermann.hpp
 	ActuatorEffectivenessRoverAckermann.cpp
-	ActuatorEffectivenessRoverDifferential.hpp
-	ActuatorEffectivenessRoverDifferential.cpp
 )
 
 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

src/modules/control_allocator/ControlAllocator.cpp

@@ -239,7 +239,7 @@ ControlAllocator::update_effectiveness_source()
 			break;
 
 		case EffectivenessSource::ROVER_DIFFERENTIAL:
-			tmp = new ActuatorEffectivenessRoverDifferential();
+			// differential_drive_control does allocation and publishes directly to actuator_motors topic
 			break;
 
 		case EffectivenessSource::FIXED_WING:

src/modules/control_allocator/ControlAllocator.hpp

@@ -47,7 +47,6 @@
 #include <ActuatorEffectivenessTiltrotorVTOL.hpp>
 #include <ActuatorEffectivenessTailsitterVTOL.hpp>
 #include <ActuatorEffectivenessRoverAckermann.hpp>
-#include <ActuatorEffectivenessRoverDifferential.hpp>
 #include <ActuatorEffectivenessFixedWing.hpp>
 #include <ActuatorEffectivenessMCTilt.hpp>
 #include <ActuatorEffectivenessCustom.hpp>

src/modules/differential_drive_control/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(DifferentialDriveKinematics)
+
+px4_add_module(
+	MODULE modules__differential_drive_control
+	MAIN differential_drive_control
+	SRCS
+		DifferentialDriveControl.cpp
+		DifferentialDriveControl.hpp
+	DEPENDS
+		DifferentialDriveKinematics
+		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
+)

src/modules/differential_drive_control/DifferentialDriveControl.cpp

@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   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 "DifferentialDriveControl.hpp"
+
+using namespace time_literals;
+using namespace matrix;
+namespace differential_drive_control
+{
+
+DifferentialDriveControl::DifferentialDriveControl() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+}
+
+bool DifferentialDriveControl::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void DifferentialDriveControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
+	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
+
+	_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
+	_differential_drive_kinematics.setMaxSpeed(_max_speed);
+	_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
+}
+
+void DifferentialDriveControl::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		updateParams();
+	}
+
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+			_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
+		}
+	}
+
+	if (_manual_driving) {
+		// Manual mode
+		// directly produce setpoints from the manual control setpoint (joystick)
+		if (_manual_control_setpoint_sub.updated()) {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+				_differential_drive_setpoint.timestamp = now;
+				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
+				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
+									_max_angular_velocity;
+				_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
+			}
+		}
+	}
+
+	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
+
+	// publish data to actuator_motors (output module)
+	// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
+	Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
+					_differential_drive_setpoint.speed,
+					_differential_drive_setpoint.yaw_rate);
+
+	// Check if max_angular_wheel_speed is zero
+	const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
+	const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
+
+	if (!_armed || setpoint_timeout || !valid_max_speed) {
+		wheel_speeds = {}; // stop
+	}
+
+	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
+
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
+	wheel_speeds.copyTo(actuator_motors.control);
+	actuator_motors.timestamp = now;
+	_actuator_motors_pub.publish(actuator_motors);
+}
+
+int DifferentialDriveControl::task_spawn(int argc, char *argv[])
+{
+	DifferentialDriveControl *instance = new DifferentialDriveControl();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int DifferentialDriveControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int DifferentialDriveControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Rover Differential Drive controller.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[])
+{
+	return DifferentialDriveControl::main(argc, argv);
+}
+
+} // namespace differential_drive_control

src/modules/differential_drive_control/DifferentialDriveControl.hpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   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
+
+// PX4 includes
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+// uORB includes
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/differential_drive_setpoint.h>
+
+// Standard library includes
+#include <math.h>
+
+// Local includes
+#include <DifferentialDriveKinematics.hpp>
+
+namespace differential_drive_control
+{
+
+class DifferentialDriveControl : public ModuleBase<DifferentialDriveControl>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	DifferentialDriveControl();
+	~DifferentialDriveControl() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+protected:
+	void updateParams() override;
+
+private:
+	void Run() override;
+
+	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
+
+	differential_drive_setpoint_s _differential_drive_setpoint{};
+	DifferentialDriveKinematics _differential_drive_kinematics{};
+
+	bool _armed = false;
+	bool _manual_driving = false;
+	float _max_speed{0.f};
+	float _max_angular_velocity{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
+		(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
+		(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
+		(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
+		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
+};
+
+} // namespace differential_drive_control

src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(DifferentialDriveKinematics
+	DifferentialDriveKinematics.cpp
+)
+
+target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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 "DifferentialDriveKinematics.hpp"
+
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
+{
+	if (_max_speed < FLT_EPSILON) {
+		return Vector2f();
+	}
+
+	linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
+	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
+
+	const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
+	float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
+
+	// Compute an initial gain
+	float gain = 1.0f;
+
+	if (combined_velocity > _max_speed) {
+		float excess_velocity = fabsf(combined_velocity - _max_speed);
+		float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
+		gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
+	}
+
+	// Apply the gain
+	linear_velocity_x *= gain;
+
+	// Calculate the left and right wheel speeds
+	return Vector2f(linear_velocity_x - rotational_velocity,
+			linear_velocity_x + rotational_velocity) / _max_speed;
+}
+

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   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 <matrix/matrix/math.hpp>
+
+/**
+ * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
+ *
+ * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
+ * given linear velocity and yaw rate.
+ */
+class DifferentialDriveKinematics
+{
+public:
+	DifferentialDriveKinematics() = default;
+	~DifferentialDriveKinematics() = default;
+
+	/**
+	 * @brief Sets the wheel base of the robot.
+	 *
+	 * @param wheel_base The distance between the centers of the wheels.
+	 */
+	void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
+
+	/**
+	 * @brief Sets the maximum speed of the robot.
+	 *
+	 * @param max_speed The maximum speed of the robot.
+	 */
+	void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
+
+	/**
+	 * @brief Sets the maximum angular speed of the robot.
+	 *
+	 * @param max_angular_speed The maximum angular speed of the robot.
+	 */
+	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
+
+	/**
+	 * @brief Computes the inverse kinematics for differential drive.
+	 *
+	 * @param linear_velocity_x Linear velocity along the x-axis.
+	 * @param yaw_rate Yaw rate of the robot.
+	 * @return matrix::Vector2f Motor velocities for the right and left motors.
+	 */
+	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
+
+private:
+	float _wheel_base{0.f};
+	float _max_speed{0.f};
+	float _max_angular_velocity{0.f};
+};

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp

@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+#include "DifferentialDriveKinematics.hpp"
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with zero linear velocity and zero yaw rate (stationary vehicle)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f());
+}
+
+
+TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(0.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with invalid parameters (zero wheel base and wheel radius)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f());
+}
+
+
+TEST(DifferentialDriveKinematicsTest, UnitCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with unit values for linear velocity and yaw rate
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f));
+}
+
+
+TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
+}
+
+
+TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
+}
+
+TEST(DifferentialDriveKinematicsTest, RandomCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
+}
+
+TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test rotating in place (zero linear velocity, non-zero yaw rate)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
+}
+
+TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test moving straight (non-zero linear velocity, zero yaw rate)
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(FLT_MIN);
+	kinematics.setMaxSpeed(FLT_MIN);
+	kinematics.setMaxAngularVelocity(FLT_MIN);
+
+	// Test with minimum possible input values
+	EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
+}

src/modules/differential_drive_control/Kconfig

@@ -0,0 +1,6 @@
+menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL
+	bool "differential_drive_control"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Enable support for control of differential drive rovers

src/modules/differential_drive_control/module.yaml

@@ -0,0 +1,55 @@
+module_name: Differential Drive Control
+
+parameters:
+  - group: Rover Differential Drive
+    definitions:
+      RDD_WHEEL_BASE:
+        description:
+          short: Wheel base
+          long: Distance from the center of the right wheel to the center of the left wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+      RDD_WHEEL_RADIUS:
+        description:
+          short: Wheel radius
+          long: Size of the wheel, half the diameter of the wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.1
+      RDD_SPEED_SCALE:
+        description:
+          short: Manual speed scale
+        type: float
+        min: 0
+        max: 1.0
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_ANG_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1.0
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_WHL_SPEED:
+        description:
+          short: Maximum wheel speed
+        type: float
+        unit: rad/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 10

src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv

@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
+10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,-1.6e-05,0.00011,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.9e-13,0,0,-2e-09,0,0,0,0,0,0,0,0,0.011,0.011,0.00093,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-2.8e-11,0,0,-4.8e-07,0,0,0,0,0,0,0,0,0.012,0.011,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0
@@ -219,7 +219,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 21690000,0.77,-0.012,-0.0018,-0.63,0.0057,0.021,0.017,0.0021,0.009,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.0002,0.0088,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.02,0.025,0.025,0.0084,0.058,0.058,0.038,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 21790000,0.77,-0.012,-0.0017,-0.63,0.0044,0.021,0.015,0.00046,0.01,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00029,0.0086,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00038,0.02,0.022,0.022,0.0082,0.049,0.049,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 21890000,0.77,-0.012,-0.0017,-0.63,0.0044,0.021,0.016,0.00091,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.0003,0.0086,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.02,0.024,0.024,0.0082,0.055,0.055,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
-21990000,0.77,-0.012,-0.0017,-0.63,0.003,0.023,0.016,-0.00047,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00035,0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.021,0.021,0.0081,0.047,0.047,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
+21990000,0.77,-0.012,-0.0017,-0.63,0.0031,0.023,0.016,-0.00047,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00035,0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.021,0.021,0.0081,0.047,0.047,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 22090000,0.77,-0.012,-0.0017,-0.63,0.0035,0.022,0.015,-0.00015,0.016,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00036,0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.023,0.023,0.0081,0.053,0.053,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 22190000,0.77,-0.012,-0.0017,-0.63,0.0038,0.019,0.015,-0.00015,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-5.3e-05,0.0084,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.021,0.021,0.008,0.046,0.046,0.037,1.1e-06,1.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 22290000,0.77,-0.012,-0.0017,-0.63,0.005,0.021,0.015,0.00028,0.015,-3.7e+02,-0.0015,-0.0061,4.4e-05,-6.1e-05,0.0084,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.022,0.022,0.008,0.051,0.051,0.037,1.1e-06,1.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0

src/modules/ekf2/test/change_indication/iris_gps.csv

@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
+10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,-3.8e-05,-0.00013,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.037,-0.00017,-0.00043,-0.017,4.7e-10,-5e-10,-1.5e-11,0,0,-6.8e-08,0,0,0,0,0,0,0,0,0.011,0.011,0.00094,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.8e-09,-5.9e-09,-4.7e-10,0,0,-2.9e-06,0,0,0,0,0,0,0,0,0.011,0.012,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -466,7 +466,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 	}
 
 	// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
-	_weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane);
+	_weathervane.setNavigatorForceDisabled(PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw));
 
 	// Calculate the current vehicle state and check if it has updated.
 	State previous_state = _current_state;
@@ -481,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
 				_triplet_next_wp,
 				_sub_triplet_setpoint.get().next.yaw,
-				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
+				(float)NAN,
 				_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(
 			_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
@@ -509,13 +509,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yaw_setpoint = NAN;
 			_yawspeed_setpoint = 0.f;
 
-		} else if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane)
-			   && _sub_triplet_setpoint.get().current.yaw_valid) {
-			// Use the yaw computed in Navigator except during takeoff because
-			// Navigator is not handling the yaw reset properly.
-			// But: use if from Navigator during takeoff if disable_weather_vane is true,
-			// because we're then aligning to the transition waypoint.
-			// TODO: fix in navigator
+		} else if (PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)) {
 			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
 			_yawspeed_setpoint = NAN;
 

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -150,7 +150,7 @@ float FixedwingAttitudeControl::get_airspeed_constrained()
 	// if no airspeed measurement is available out best guess is to use the trim airspeed
 	float airspeed = _param_fw_airspd_trim.get();
 
-	if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
+	if (_param_fw_use_airspd.get() && airspeed_valid) {
 		/* prevent numerical drama by requiring 0.5 m/s minimal speed */
 		airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);
 

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -135,7 +135,7 @@ private:
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
-		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
 
 		(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
 		(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -75,6 +75,7 @@ bool FwAutotuneAttitudeControl::init()
 
 void FwAutotuneAttitudeControl::reset()
 {
+	_param_fw_at_start.reset();
 }
 
 void FwAutotuneAttitudeControl::Run()

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -59,9 +59,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	, _figure_eight(_npfg, _wind_vel, _eas2tas)
 #endif // CONFIG_FIGURE_OF_EIGHT
 {
-	if (vtol) {
-		_param_handle_airspeed_trans = param_find("VT_ARSP_TRANS");
-	}
 
 	// limit to 50 Hz
 	_local_pos_sub.set_interval_ms(20);
@@ -80,7 +77,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	/* fetch initial parameter values */
 	parameters_update();
 
-	_airspeed_slew_rate_controller.setForcedValue(_performance_model.getCalibratedTrimAirspeed());
 	_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
 	_roll_slew_rate.setForcedValue(0.f);
 
@@ -111,11 +107,6 @@ FixedwingPositionControl::parameters_update()
 
 	_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
 
-	// VTOL parameter VT_ARSP_TRANS
-	if (_param_handle_airspeed_trans != PARAM_INVALID) {
-		param_get(_param_handle_airspeed_trans, &_param_airspeed_trans);
-	}
-
 	// NPFG parameters
 	_npfg.setPeriod(_param_npfg_period.get());
 	_npfg.setDamping(_param_npfg_damping.get());
@@ -214,20 +205,23 @@ FixedwingPositionControl::airspeed_poll()
 	bool airspeed_valid = _airspeed_valid;
 	airspeed_validated_s airspeed_validated;
 
-	if ((_param_fw_arsp_mode.get() == 0) && _airspeed_validated_sub.update(&airspeed_validated)) {
+	if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {
 
 		_eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed
 
 		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
 		    && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
-		    && (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) {
+		    && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) {
 
 			airspeed_valid = true;
 
 			_time_airspeed_last_valid = airspeed_validated.timestamp;
-			_airspeed = airspeed_validated.calibrated_airspeed_m_s;
+			_airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
 
 			_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
+
+		} else {
+			airspeed_valid = false;
 		}
 
 	} else {
@@ -391,8 +385,15 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 	calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
 				       _performance_model.getMaximumCalibratedAirspeed());
 
-	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) {
-		_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
+	// initialize the airspeed setpoint to the max(current airsped, min airspeed)
+	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())
+	    || !_tecs_is_running) {
+		_airspeed_slew_rate_controller.setForcedValue(math::max(calibrated_min_airspeed, _airspeed_eas));
+	}
+
+	// reset the airspeed setpoint to the min airspeed if the min airspeed changes while in operation
+	if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) {
+		_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
 	}
 
 	if (control_interval > FLT_EPSILON) {
@@ -400,10 +401,6 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 		_airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);
 	}
 
-	if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) {
-		_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
-	}
-
 	if (_airspeed_slew_rate_controller.getState() > _performance_model.getMaximumCalibratedAirspeed()) {
 		_airspeed_slew_rate_controller.setForcedValue(_performance_model.getMaximumCalibratedAirspeed());
 	}
@@ -648,7 +645,7 @@ void
 FixedwingPositionControl::updateManualTakeoffStatus()
 {
 	if (!_completed_manual_takeoff) {
-		const bool at_controllable_airspeed = _airspeed > _performance_model.getMinimumCalibratedAirspeed(getLoadFactor())
+		const bool at_controllable_airspeed = _airspeed_eas > _performance_model.getMinimumCalibratedAirspeed(getLoadFactor())
 						      || !_airspeed_valid;
 		const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 					 && _control_mode.flag_armed;
@@ -1372,7 +1369,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_runway_takeoff.forceSetFlyState();
 		}
 
-		_runway_takeoff.update(now, takeoff_airspeed, _airspeed, _current_altitude - _takeoff_ground_alt,
+		_runway_takeoff.update(now, takeoff_airspeed, _airspeed_eas, _current_altitude - _takeoff_ground_alt,
 				       clearance_altitude_amsl - _takeoff_ground_alt);
 
 		// yaw control is disabled once in "taking off" state
@@ -2516,48 +2513,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		const float desired_max_sinkrate, const float desired_max_climbrate,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
-	_tecs_is_running = true;
-
 	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
-	// (it should also not run during VTOL blending because airspeed is too low still)
-	if (_vehicle_status.is_vtol) {
-		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
-			_tecs_is_running = false;
-		}
-
-		if (_vehicle_status.in_transition_mode) {
-			// we're in transition
-			_was_in_transition = true;
-
-			// set this to transition airspeed to init tecs correctly
-			if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
-				// some vtols fly without airspeed sensor
-				_airspeed_after_transition = _param_airspeed_trans;
-
-			} else {
-				_airspeed_after_transition = _airspeed;
-			}
-
-			_airspeed_after_transition = constrain(_airspeed_after_transition,
-							       _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()),
-							       _performance_model.getMaximumCalibratedAirspeed());
-
-		} else if (_was_in_transition) {
-			// after transition we ramp up desired airspeed from the speed we had coming out of the transition
-			_airspeed_after_transition += control_interval * 2.0f; // increase 2m/s
-
-			if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
-				airspeed_sp = max(_airspeed_after_transition, _airspeed);
-
-			} else {
-				_was_in_transition = false;
-				_airspeed_after_transition = 0.0f;
-			}
-		}
-	}
-
-	if (!_tecs_is_running) {
+	if (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+					|| _vehicle_status.in_transition_mode)) {
+		_tecs_is_running = false;
 		return;
+
+	} else {
+		_tecs_is_running = true;
 	}
 
 	/* update TECS vehicle state estimates */
@@ -2575,7 +2538,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     _current_altitude,
 		     alt_sp,
 		     airspeed_sp,
-		     _airspeed,
+		     _airspeed_eas,
 		     _eas2tas,
 		     throttle_min,
 		     throttle_max,

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -360,8 +360,8 @@ private:
 
 	// AIRSPEED
 
-	float _airspeed{0.0f};
-	float _eas2tas{1.0f};
+	float _airspeed_eas{0.f};
+	float _eas2tas{1.f};
 	bool _airspeed_valid{false};
 	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};
 
@@ -384,13 +384,8 @@ private:
 
 	bool _tecs_is_running{false};
 	// VTOL / TRANSITION
-
-	float _airspeed_after_transition{0.0f};
-	bool _was_in_transition{false};
 	bool _is_vtol_tailsitter{false};
 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
-	param_t _param_handle_airspeed_trans{PARAM_INVALID};
-	float _param_airspeed_trans{NAN}; // [m/s]
 
 	// ESTIMATOR RESET COUNTERS
 
@@ -959,7 +954,7 @@ private:
 		(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,
 
 		// external parameters
-		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
 
 		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
 

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -162,7 +162,7 @@ float FixedwingRateControl::get_airspeed_and_update_scaling()
 	// if no airspeed measurement is available out best guess is to use the trim airspeed
 	float airspeed = _param_fw_airspd_trim.get();
 
-	if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
+	if (_param_fw_use_airspd.get() && airspeed_valid) {
 		/* prevent numerical drama by requiring 0.5 m/s minimal speed */
 		airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);
 

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -164,7 +164,7 @@ private:
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
-		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
 
 		(ParamInt<px4::params::FW_ARSP_SCALE_EN>) _param_fw_arsp_scale_en,
 

src/modules/fw_rate_control/fw_rate_control_params.c

@@ -41,16 +41,18 @@
  */
 
 /**
- * Airspeed mode
+ * Use airspeed for control
  *
- * On vehicles without airspeed sensor this parameter can be used to
- * enable flying without an airspeed reading
+ * If set to 1, the airspeed measurement data, if valid, is used in the following controllers:
+ * - Rate controller: output scaling
+ * - Attitude controller: coordinated turn controller
+ * - Position controller: airspeed setpoint tracking, takeoff logic
+ * - VTOL: transition logic
  *
- * @value 0 Use airspeed in controller
- * @value 1 Do not use airspeed in controller
+ * @boolean
  * @group FW Rate Control
  */
-PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
+PARAM_DEFINE_INT32(FW_USE_AIRSPD, 1);
 
 /**
  * Pitch rate proportional gain.

src/modules/logger/logged_topics.cpp

@@ -57,6 +57,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("commander_state");
 	add_topic("config_overrides");
 	add_topic("cpuload");
+	add_optional_topic("differential_drive_setpoint", 100);
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
 	add_optional_topic("external_ins_local_position");

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp

@@ -68,6 +68,7 @@ bool McAutotuneAttitudeControl::init()
 
 void McAutotuneAttitudeControl::reset()
 {
+	_param_mc_at_start.reset();
 }
 
 void McAutotuneAttitudeControl::Run()

src/modules/navigator/mission.cpp

@@ -322,8 +322,7 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
 
 		_mission_item.lat = _global_pos_sub.get().lat;
 		_mission_item.lon = _global_pos_sub.get().lon;
-		/* hold heading for takeoff items */
-		_mission_item.yaw = _navigator->get_local_position()->heading;
+		_mission_item.yaw = NAN; // FlightTaskAuto handles yaw directly
 		_mission_item.altitude = _mission_init_climb_altitude_amsl;
 		_mission_item.altitude_is_relative = false;
 		_mission_item.autocontinue = true;
@@ -366,9 +365,6 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
 	    _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 	    !_land_detected_sub.get().landed) {
 
-		/* disable weathervane before front transition for allowing yaw to align */
-		pos_sp_triplet->current.disable_weather_vane = true;
-
 		/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
 		_mission_item.yaw = get_bearing_to_next_waypoint(
 					    _global_pos_sub.get().lat, _global_pos_sub.get().lon,
@@ -389,16 +385,13 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
 	    _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 	    !_land_detected_sub.get().landed) {
 
-		/* re-enable weather vane again after alignment */
-		pos_sp_triplet->current.disable_weather_vane = false;
-
 		/* check if the vtol_takeoff waypoint is on top of us */
 		if (do_need_move_to_takeoff()) {
 			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
 		}
 
 		set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
-		_mission_item.yaw = _navigator->get_local_position()->heading;
+		_mission_item.yaw = NAN;
 
 		// keep current setpoints (FW position controller generates wp to track during transition)
 		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
@@ -428,9 +421,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
 	    && !_land_detected_sub.get().landed
 	    && (num_found_items > 0u)) {
 
-		/* disable weathervane before front transition for allowing yaw to align */
-		pos_sp_triplet->current.disable_weather_vane = true;
-
 		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING;
 
 		set_align_mission_item(&_mission_item, &next_mission_items[0u]);
@@ -445,9 +435,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
 
 		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT;
 
-		/* re-enable weather vane again after alignment */
-		pos_sp_triplet->current.disable_weather_vane = false;
-
 		pos_sp_triplet->previous = pos_sp_triplet->current;
 		// keep current setpoints (FW position controller generates wp to track during transition)
 		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;

src/modules/navigator/mission_base.cpp

@@ -654,9 +654,6 @@ bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const p
 		(fabs(p1->lon - p2->lon) < DBL_EPSILON) &&
 		(fabsf(p1->alt - p2->alt) < FLT_EPSILON) &&
 		((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) &&
-		(p1->yaw_valid == p2->yaw_valid) &&
-		(fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) &&
-		(p1->yawspeed_valid == p2->yawspeed_valid) &&
 		(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
 		(p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) &&
 		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
@@ -773,13 +770,11 @@ MissionBase::heading_sp_update()
 
 			_mission_item.yaw = yaw;
 			pos_sp_triplet->current.yaw = _mission_item.yaw;
-			pos_sp_triplet->current.yaw_valid = true;
 
 		} else {
-			if (!pos_sp_triplet->current.yaw_valid) {
-				_mission_item.yaw = _navigator->get_local_position()->heading;
+			if (!PX4_ISFINITE(pos_sp_triplet->current.yaw)) {
+				_mission_item.yaw = NAN;
 				pos_sp_triplet->current.yaw = _mission_item.yaw;
-				pos_sp_triplet->current.yaw_valid = true;
 			}
 		}
 

src/modules/navigator/mission_block.cpp

@@ -411,7 +411,7 @@ MissionBlock::is_mission_item_reached_or_completed()
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-		    && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))
+		    && _navigator->get_yaw_to_be_accepted(_mission_item.yaw)
 		    && _navigator->get_local_position()->heading_good_for_control) {
 
 			const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading);
@@ -423,14 +423,6 @@ MissionBlock::is_mission_item_reached_or_completed()
 				_waypoint_yaw_reached = true;
 			}
 
-			// Always accept yaw during takeoff
-			// TODO: Ideally Navigator would handle a yaw reset and adjust its yaw setpoint, making the
-			// following no longer necessary.
-			// FlightTaskAuto is currently also ignoring the yaw setpoint during takeoff and thus "handling" it.
-			if (_mission_item.nav_cmd == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
-				_waypoint_yaw_reached = true;
-			}
-
 			/* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */
 			if (!_waypoint_yaw_reached && _mission_item.force_heading &&
 			    (_navigator->get_yaw_timeout() >= FLT_EPSILON) &&
@@ -473,8 +465,8 @@ MissionBlock::is_mission_item_reached_or_completed()
 							 && curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER
 							 && (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);
 
-			// can only enforce exit course if next waypoint is not within loiter radius of current waypoint
-			const bool exit_course_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
+			// can only enforce exit course if next waypoint is not within loiter radius of current waypoint (with small margin)
+			const bool exit_course_is_reachable = dist_current_next > 1.05f * curr_sp_new->loiter_radius;
 
 			if (enforce_exit_course && exit_course_is_reachable) {
 
@@ -668,7 +660,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->lon = item.lon;
 	sp->alt = get_absolute_altitude_for_item(item);
 	sp->yaw = item.yaw;
-	sp->yaw_valid = PX4_ISFINITE(item.yaw);
 	sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
 			    _navigator->get_loiter_radius();
 	sp->loiter_direction_counter_clockwise = item.loiter_radius < 0;
@@ -704,6 +695,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 
 		} else {
 			sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
+
+			// Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset.
+			// The yaw setpoint generation is handled by FlightTaskAuto.
+			sp->yaw = NAN;
 		}
 
 		break;
@@ -746,6 +741,7 @@ MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *it
 	item->altitude = pos_sp_triplet->current.alt;
 	item->loiter_radius = pos_sp_triplet->current.loiter_direction_counter_clockwise ?
 			      -pos_sp_triplet->current.loiter_radius : pos_sp_triplet->current.loiter_radius;
+	item->yaw = pos_sp_triplet->current.yaw;
 }
 
 void
@@ -765,8 +761,8 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
 	}
 
 	item->altitude = loiter_altitude_amsl;
-
 	item->loiter_radius = _navigator->get_loiter_radius();
+	item->yaw = NAN;
 }
 
 void
@@ -774,10 +770,11 @@ MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s
 {
 	setLoiterItemCommonFields(item);
 
-	_navigator->calculate_breaking_stop(item->lat, item->lon, item->yaw);
+	_navigator->calculate_breaking_stop(item->lat, item->lon);
 
 	item->altitude = _navigator->get_global_position()->alt;
 	item->loiter_radius = _navigator->get_loiter_radius();
+	item->yaw = NAN;
 }
 
 void
@@ -801,7 +798,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
 	/* use current position */
 	item->lat = _navigator->get_global_position()->lat;
 	item->lon = _navigator->get_global_position()->lon;
-	item->yaw = _navigator->get_local_position()->heading;
+	item->yaw = NAN;
 
 	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
@@ -831,7 +828,7 @@ MissionBlock::set_land_item(struct mission_item_s *item)
 	// set land item to current position
 	item->lat = _navigator->get_global_position()->lat;
 	item->lon = _navigator->get_global_position()->lon;
-	item->yaw = _navigator->get_local_position()->heading;
+	item->yaw = NAN;
 
 	item->altitude = 0;
 	item->altitude_is_relative = false;
@@ -863,14 +860,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
 {
 	item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 	item->params[0] = (float) new_mode;
-	item->params[1] = 0.0f;
-
-	// Keep yaw from previous mission item if valid, as that is containing the transition heading.
-	// If not valid use current yaw as yaw setpoint
-	if (!PX4_ISFINITE(item->yaw)) {
-		item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading
-	}
-
+	item->params[1] = 0.0f; // not immediate transition
 	item->autocontinue = true;
 }
 

src/modules/navigator/navigator.h

@@ -239,14 +239,13 @@ public:
 	void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
 
 	/**
-	 * Get the yaw acceptance given the current mission item
+	 * Get if the yaw acceptance is required at the current mission item
 	 *
 	 * @param mission_item_yaw the yaw to use in case the controller-derived radius is finite
 	 *
-	 * @return the yaw at which the next waypoint should be used or NaN if the yaw at a waypoint
-	 * should be ignored
+	 * @return true if the yaw acceptance is required, false if not required
 	 */
-	float get_yaw_acceptance(float mission_item_yaw);
+	bool get_yaw_to_be_accepted(float mission_item_yaw);
 
 	orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
 
@@ -279,7 +278,7 @@ public:
 	void release_gimbal_control();
 	void set_gimbal_neutral();
 
-	void calculate_breaking_stop(double &lat, double &lon, float &yaw);
+	void calculate_breaking_stop(double &lat, double &lon);
 
 	void stop_capturing_images();
 	void disable_camera_trigger();

src/modules/navigator/navigator_main.cpp

@@ -309,11 +309,9 @@ void Navigator::run()
 					// Go on and check which changes had been requested
 					if (PX4_ISFINITE(cmd.param4)) {
 						rep->current.yaw = cmd.param4;
-						rep->current.yaw_valid = true;
 
 					} else {
 						rep->current.yaw = NAN;
-						rep->current.yaw_valid = false;
 					}
 
 					if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
@@ -348,8 +346,7 @@ void Navigator::run()
 						if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 						    && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
 
-							calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
-							rep->current.yaw_valid = true;
+							calculate_breaking_stop(rep->current.lat, rep->current.lon);
 
 						} else {
 							// For fixedwings we can use the current vehicle's position to define the loiter point
@@ -446,7 +443,6 @@ void Navigator::run()
 					rep->current.cruising_throttle = get_cruising_throttle();
 					rep->current.acceptance_radius = get_acceptance_radius();
 					rep->current.yaw = NAN;
-					rep->current.yaw_valid = false;
 
 					// Position is not changing, thus we keep the setpoint
 					rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat;
@@ -458,8 +454,7 @@ void Navigator::run()
 					if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 					    && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
 
-						calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
-						rep->current.yaw_valid = true;
+						calculate_breaking_stop(rep->current.lat, rep->current.lon);
 					}
 
 					if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
@@ -599,19 +594,18 @@ void Navigator::run()
 				rep->current.cruising_speed = -1.f; // reset to default
 
 				if (home_global_position_valid()) {
-					// Only set yaw if we know the true heading
-					// We assume that the heading is valid when the global position is valid because true heading
-					// is required to fuse NE (e.g.: GNSS) data. // TODO: we should be more explicit here
-					rep->current.yaw = cmd.param4;
 
 					rep->previous.valid = true;
 					rep->previous.timestamp = hrt_absolute_time();
 
 				} else {
-					rep->current.yaw = get_local_position()->heading;
 					rep->previous.valid = false;
 				}
 
+				// Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset.
+				// The yaw setpoint generation is handled by FlightTaskAuto.
+				rep->current.yaw = NAN;
+
 				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
 					rep->current.lat = cmd.param5;
 					rep->current.lon = cmd.param6;
@@ -1039,8 +1033,7 @@ void Navigator::geofence_breach_check()
 				}
 
 				rep->current.timestamp = hrt_absolute_time();
-				rep->current.yaw = get_local_position()->heading;
-				rep->current.yaw_valid = true;
+				rep->current.yaw = NAN;
 				rep->current.lat = loiter_latitude;
 				rep->current.lon = loiter_longitude;
 				rep->current.alt = loiter_altitude_amsl;
@@ -1157,13 +1150,13 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp)
 	sp.timestamp = hrt_absolute_time();
 	sp.lat = static_cast<double>(NAN);
 	sp.lon = static_cast<double>(NAN);
+	sp.yaw = NAN;
 	sp.loiter_radius = get_loiter_radius();
 	sp.acceptance_radius = get_default_acceptance_radius();
 	sp.cruising_speed = get_cruising_speed();
 	sp.cruising_throttle = get_cruising_throttle();
 	sp.valid = false;
 	sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
-	sp.disable_weather_vane = false;
 	sp.loiter_direction_counter_clockwise = false;
 }
 
@@ -1193,7 +1186,7 @@ float Navigator::get_acceptance_radius()
 	return acceptance_radius;
 }
 
-float Navigator::get_yaw_acceptance(float mission_item_yaw)
+bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw)
 {
 	float yaw = mission_item_yaw;
 
@@ -1205,7 +1198,7 @@ float Navigator::get_yaw_acceptance(float mission_item_yaw)
 		yaw = pos_ctrl_status.yaw_acceptance;
 	}
 
-	return yaw;
+	return PX4_ISFINITE(yaw);
 }
 
 void Navigator::load_fence_from_file(const char *filename)
@@ -1470,21 +1463,20 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
 	return true;
 }
 
-void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
+void Navigator::calculate_breaking_stop(double &lat, double &lon)
 {
 	// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back
-	float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
+	const float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
 
 	// predict braking distance
 
 	const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
 
-	float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
-					    _param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto);
+	const float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
+			_param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto);
 
 	waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground,
 					   multirotor_braking_distance, &lat, &lon);
-	yaw = get_local_position()->heading;
 }
 
 void Navigator::mode_completed(uint8_t nav_state, uint8_t result)

src/modules/navigator/takeoff.cpp

@@ -130,7 +130,6 @@ Takeoff::set_takeoff_position()
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 	pos_sp_triplet->previous.valid = false;
-	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;
 
 	if (rep->current.valid) {

src/modules/navigator/vtol_takeoff.cpp

@@ -75,7 +75,6 @@ VtolTakeoff::on_active()
 							    _mission_item.lon, _loiter_location(0), _loiter_location(1)));
 				_mission_item.force_heading = true;
 				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
-				pos_sp_triplet->current.disable_weather_vane = true;
 				pos_sp_triplet->current.cruising_speed = -1.f;
 				_navigator->set_position_setpoint_triplet_updated();
 
@@ -182,7 +181,6 @@ VtolTakeoff::set_takeoff_position()
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 	pos_sp_triplet->previous.valid = false;
-	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();

src/modules/sensors/vehicle_gps_position/gps_blending.cpp

@@ -76,7 +76,8 @@ void GpsBlending::update(uint64_t hrt_now_us)
 		// Only use a secondary instance if the fallback is allowed
 		if ((_primary_instance > -1)
 		    && (gps_select_index != _primary_instance)
-		    && !_fallback_allowed) {
+		    && _primary_instance_available
+		    && (_gps_state[_primary_instance].fix_type >= 3)) {
 			gps_select_index = _primary_instance;
 		}
 
@@ -87,6 +88,10 @@ void GpsBlending::update(uint64_t hrt_now_us)
 			_gps_updated[gps_select_index] = false;
 		}
 	}
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
+		_time_prev_us[i] = _gps_state[i].timestamp;
+	}
 }
 
 bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
@@ -121,6 +126,10 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
 		if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) {
 			_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
 
+			if (i == _primary_instance) {
+				_primary_instance_available = true;
+			}
+
 		} else if ((present_dt >= GPS_TIMEOUT_S) && (_gps_state[i].timestamp > 0)) {
 			// Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt
 			_gps_state[i].timestamp = 0;
@@ -129,9 +138,8 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
 			_gps_state[i].vel_ned_valid = 0;
 
 			if (i == _primary_instance) {
-				// Allow using a secondary instance when the primary
-				// receiver has timed out
-				_fallback_allowed = true;
+				// Allow using a secondary instance when the primary receiver has timed out
+				_primary_instance_available = false;
 			}
 
 			continue;
@@ -523,8 +531,6 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
 			// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
 			alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
 					     0.0f, 1.0f);
-
-			_time_prev_us[i] = _gps_state[i].timestamp;
 		}
 	}
 

src/modules/sensors/vehicle_gps_position/gps_blending.hpp

@@ -126,7 +126,7 @@ private:
 	int _selected_gps{0};
 	int _np_gps_suitable_for_blending{0};
 	int _primary_instance{0}; ///< if -1, there is no primary isntance and the best receiver is used // TODO: use device_id
-	bool _fallback_allowed{false};
+	bool _primary_instance_available{false};
 
 	bool _is_new_output_data_available{false};
 

src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp

@@ -235,16 +235,14 @@ TEST_F(GpsBlendingTest, dualReceiverFailover)
 	const float duration_s = 10.f;
 	runSeconds(duration_s, gps_blending, gps_data1, 1);
 
-	// THEN: the primary instance should be selected even if
-	// not available. No data is then available
-	EXPECT_EQ(gps_blending.getSelectedGps(), 0);
+	// THEN: the secondary instance as the primary one is not available
+	EXPECT_EQ(gps_blending.getSelectedGps(), 1);
 	EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1);
-	EXPECT_FALSE(gps_blending.isNewOutputDataAvailable());
+	EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
 
 	// BUT WHEN: the data of the primary receiver is avaialbe
 	sensor_gps_s gps_data0 = getDefaultGpsData();
-	gps_blending.setGpsData(gps_data0, 0);
-	gps_blending.update(_time_now_us);
+	runSeconds(1.f, gps_blending, gps_data0, gps_data1);
 
 	// THEN: the primary instance is selected and the data
 	// is available
@@ -274,6 +272,24 @@ TEST_F(GpsBlendingTest, dualReceiverFailover)
 	// THEN: the primary receiver should be used again
 	EXPECT_EQ(gps_blending.getSelectedGps(), 0);
 	EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
+
+	// BUT IF: the secondary receiver has better metrics than the primary one
+	gps_data1.satellites_used = gps_data0.satellites_used + 2;
+
+	runSeconds(1.f, gps_blending, gps_data0, gps_data1);
+
+	// THEN: the selector shouldn't switch again as the primary one is available
+	EXPECT_EQ(gps_blending.getSelectedGps(), 0);
+	EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
+
+	// BUT IF: the primary receiver looses its fix
+	gps_data0.fix_type = 1;
+
+	runSeconds(1.f, gps_blending, gps_data0, gps_data1);
+
+	// THEN: the selector should switch as the primary one is unable to provide correct data
+	EXPECT_EQ(gps_blending.getSelectedGps(), 1);
+	EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
 }
 
 TEST_F(GpsBlendingTest, dualReceiverUTCTime)

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -64,6 +64,8 @@ if(gz-transport_FOUND)
 			GZMixingInterfaceESC.hpp
 			GZMixingInterfaceServo.cpp
 			GZMixingInterfaceServo.hpp
+			GZMixingInterfaceWheel.cpp
+			GZMixingInterfaceWheel.hpp
 		DEPENDS
 			mixer_module
 			px4_work_queue

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -226,6 +226,11 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	if (!_mixing_interface_wheel.init(_model_name)) {
+		PX4_ERR("failed to init motor output");
+		return PX4_ERROR;
+	}
+
 	ScheduleNow();
 	return OK;
 }
@@ -724,6 +729,7 @@ void GZBridge::Run()
 
 		_mixing_interface_esc.stop();
 		_mixing_interface_servo.stop();
+		_mixing_interface_wheel.stop();
 
 		exit_and_cleanup();
 		return;
@@ -739,6 +745,7 @@ void GZBridge::Run()
 
 		_mixing_interface_esc.updateParams();
 		_mixing_interface_servo.updateParams();
+		_mixing_interface_wheel.updateParams();
 	}
 
 	ScheduleDelayed(10_ms);
@@ -754,6 +761,9 @@ int GZBridge::print_status()
 	PX4_INFO_RAW("Servo outputs:\n");
 	_mixing_interface_servo.mixingOutput().printStatus();
 
+	PX4_INFO_RAW("Wheel outputs:\n");
+	_mixing_interface_wheel.mixingOutput().printStatus();
+
 	return 0;
 }
 

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -35,6 +35,7 @@
 
 #include "GZMixingInterfaceESC.hpp"
 #include "GZMixingInterfaceServo.hpp"
+#include "GZMixingInterfaceWheel.hpp"
 
 #include <px4_platform_common/atomic.h>
 #include <px4_platform_common/defines.h>
@@ -56,6 +57,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/wheel_encoders.h>
 
 #include <gz/math.hh>
 #include <gz/msgs.hh>
@@ -63,6 +65,7 @@
 
 #include <gz/msgs/imu.pb.h>
 #include <gz/msgs/fluid_pressure.pb.h>
+#include <gz/msgs/model.pb.h>
 #include <gz/msgs/odometry_with_covariance.pb.h>
 
 using namespace time_literals;
@@ -129,6 +132,7 @@ private:
 
 	GZMixingInterfaceESC   _mixing_interface_esc{_node, _node_mutex};
 	GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
+	GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex};
 
 	px4::atomic<uint64_t> _world_time_us{0};
 

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   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 "GZMixingInterfaceWheel.hpp"
+
+bool GZMixingInterfaceWheel::init(const std::string &model_name)
+{
+
+	std::string wheel_speed_topic = "/model/" + model_name + "/command/motor_speed";
+
+	if (!_node.Subscribe(wheel_speed_topic, &GZMixingInterfaceWheel::wheelSpeedCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", wheel_speed_topic.c_str());
+		return false;
+	}
+
+	std::string wheel_topic = "/model/" + model_name + "/command/motor_speed";
+	_actuators_pub = _node.Advertise<gz::msgs::Actuators>(wheel_topic);
+
+	if (!_actuators_pub.Valid()) {
+		PX4_ERR("failed to advertise %s", wheel_topic.c_str());
+		return false;
+	}
+
+	_wheel_encoders_pub.advertise();
+
+	ScheduleNow();
+
+	return true;
+}
+
+bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+		unsigned num_control_groups_updated)
+{
+	unsigned active_output_count = 0;
+
+	for (unsigned i = 0; i < num_outputs; i++) {
+		if (_mixing_output.isFunctionSet(i)) {
+			active_output_count++;
+
+		} else {
+			break;
+		}
+	}
+
+	if (active_output_count > 0) {
+		gz::msgs::Actuators wheel_velocity_message;
+		wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
+
+		for (unsigned i = 0; i < active_output_count; i++) {
+			// Offsetting the output allows for negative values despite unsigned integer to reverse the wheels
+			static constexpr float output_offset = 100.0f;
+			float scaled_output = (float)outputs[i] - output_offset;
+			wheel_velocity_message.set_velocity(i, scaled_output);
+		}
+
+
+		if (_actuators_pub.Valid()) {
+			return _actuators_pub.Publish(wheel_velocity_message);
+		}
+	}
+
+	return false;
+}
+
+void GZMixingInterfaceWheel::Run()
+{
+	pthread_mutex_lock(&_node_mutex);
+	_mixing_output.update();
+	_mixing_output.updateSubscriptions(false);
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+void GZMixingInterfaceWheel::wheelSpeedCallback(const gz::msgs::Actuators &actuators)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_node_mutex);
+
+	wheel_encoders_s wheel_encoders{};
+
+	for (int i = 0; i < actuators.velocity_size(); i++) {
+		// Convert from RPM to rad/s
+		wheel_encoders.wheel_speed[i] = (float)actuators.velocity(i) * (2.0f * M_PI_F / 60.0f);
+	}
+
+	if (actuators.velocity_size() > 0) {
+		wheel_encoders.timestamp = hrt_absolute_time();
+		_wheel_encoders_pub.publish(wheel_encoders);
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp

@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   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 <lib/mixer_module/mixer_module.hpp>
+
+#include <gz/msgs.hh>
+#include <gz/transport.hh>
+
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/wheel_encoders.h>
+
+
+// GZBridge mixing class for Wheels.
+// It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling
+// All work items are expected to run on the same work queue.
+class GZMixingInterfaceWheel : public OutputModuleInterface
+{
+public:
+	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
+
+	GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
+		OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl),
+		_node(node),
+		_node_mutex(node_mutex)
+	{}
+
+	bool updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS],
+			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+
+	MixingOutput &mixingOutput() { return _mixing_output; }
+
+	bool init(const std::string &model_name);
+
+	void stop()
+	{
+		_mixing_output.unregister();
+		ScheduleClear();
+	}
+
+private:
+	friend class GZBridge;
+
+	void Run() override;
+
+	void wheelSpeedCallback(const gz::msgs::Actuators &actuators);
+
+	gz::transport::Node &_node;
+	pthread_mutex_t &_node_mutex;
+
+	MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+
+	gz::transport::Node::Publisher _actuators_pub;
+
+	uORB::Publication<wheel_encoders_s> _wheel_encoders_pub{ORB_ID(wheel_encoders)};
+};

src/modules/simulation/gz_bridge/module.yaml

@@ -25,3 +25,12 @@ actuator_output:
         max: { min: 0, max: 1000, default: 1000 }
         failsafe: { min: 0, max: 1000 }
       num_channels: 8
+    - param_prefix: SIM_GZ_WH
+      group_label: 'Wheels'
+      channel_label: 'Wheels'
+      standard_params:
+        disarmed: { min: 0, max: 200, default: 100 }
+        min: { min: 0, max: 200, default: 0 }
+        max: { min: 0, max: 200, default: 200 }
+        failsafe: { min: 0, max: 200 }
+      num_channels: 2

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -127,6 +127,9 @@ subscriptions:
   - topic: /fmu/in/vehicle_rates_setpoint
     type: px4_msgs::msg::VehicleRatesSetpoint
 
+  - topic: /fmu/in/differential_drive_setpoint
+    type: px4_msgs::msg::DifferentialDriveSetpoint
+
   - topic: /fmu/in/vehicle_visual_odometry
     type: px4_msgs::msg::VehicleOdometry
 

src/modules/vtol_att_control/standard.cpp

@@ -217,7 +217,7 @@ void Standard::update_transition_state()
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
-		} else if (_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
+		} else if (!_param_fw_use_airspd.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
 			mc_weight = 1.0f - _time_since_trans_start / getMinimumFrontTransitionTime();
 			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
 

src/modules/vtol_att_control/tailsitter.cpp

@@ -324,7 +324,7 @@ void Tailsitter::fill_actuator_outputs()
 bool Tailsitter::isFrontTransitionCompletedBase()
 {
 	const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
-			&& !_param_fw_arsp_mode.get() ;
+			&& _param_fw_use_airspd.get();
 
 	bool transition_to_fw = false;
 	const float pitch = Eulerf(Quatf(_v_att->q)).theta();

src/modules/vtol_att_control/tiltrotor.cpp

@@ -243,7 +243,7 @@ void Tiltrotor::update_transition_state()
 		_mc_roll_weight = 1.0f;
 		_mc_yaw_weight = 1.0f;
 
-		if (!_param_fw_arsp_mode.get()  && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
+		if (_param_fw_use_airspd.get()  && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
 		    _airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed()) {
 			const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) /
 					     (getTransitionAirspeed()  - getBlendAirspeed());
@@ -252,7 +252,7 @@ void Tiltrotor::update_transition_state()
 		}
 
 		// without airspeed do timed weight changes
-		if ((_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
+		if ((!_param_fw_use_airspd.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
 		    _time_since_trans_start > getMinimumFrontTransitionTime()) {
 			_mc_roll_weight = 1.0f - (_time_since_trans_start - getMinimumFrontTransitionTime()) /
 					  (getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());

src/modules/vtol_att_control/vtol_type.cpp

@@ -208,7 +208,7 @@ bool VtolType::isFrontTransitionCompletedBase()
 {
 	// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
 	const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
-			&& !_param_fw_arsp_mode.get();
+			&& _param_fw_use_airspd.get();
 	const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime();
 	const bool openloop_trans_time_elapsed = _time_since_trans_start > getOpenLoopFrontTransitionTime();
 

src/modules/vtol_att_control/vtol_type.h

@@ -354,7 +354,7 @@ protected:
 					(ParamFloat<px4::params::VT_ARSP_TRANS>) _param_vt_arsp_trans,
 					(ParamFloat<px4::params::VT_F_TRANS_THR>) _param_vt_f_trans_thr,
 					(ParamFloat<px4::params::VT_ARSP_BLEND>) _param_vt_arsp_blend,
-					(ParamBool<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
+					(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
 					(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout,
 					(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
 					(ParamInt<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,