Rover module: Temp

Add differential drive control module to all targets that have rover support
Differential Rover: PR fixes
2026-05-24 02:07:35 +08:00 · 2023-12-19 15:04:34 +01:00 · 2023-12-19 11:55:52 +01:00 · 2023-12-19 11:54:41 +01:00 · 2023-12-19 11:39:36 +01:00 · 2023-12-19 11:37:08 +01:00
121 changed files with 725 additions and 2009 deletions
@@ -66,6 +66,7 @@ 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
@@ -153,6 +153,7 @@ 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
@@ -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 SYS_HAS_NUM_ASPD 0
+param set-default CBRK_AIRSPD_CHK 162128
 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_USE_AIRSPD 0
+param set-default FW_ARSP_MODE 1
 param set-default NPFG_PERIOD 25
 param set-default FW_PR_FF 0.7
 param set-default FW_PR_I 0.18
@@ -54,5 +54,3 @@ 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
@@ -10,6 +10,9 @@ 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
@@ -5,7 +5,7 @@
 ekf2 start &

 # Start rover differential drive controller.
-differential_drive_control start
+rover_control start

 # Start Land Detector.
 land_detector start rover
@@ -9,3 +9,6 @@ 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
@@ -42,6 +42,4 @@ 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
@@ -0,0 +1,20 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/humble/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}
@@ -39,6 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -39,6 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -36,6 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -25,6 +25,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -43,6 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -44,6 +44,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -39,6 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -27,6 +27,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -32,6 +32,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
@@ -31,6 +31,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
@@ -32,6 +32,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
@@ -43,6 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -41,6 +41,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -3,7 +3,6 @@ CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_BOARD_ROOTFSDIR="/data/px4"
 CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
-CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_QSHELL_POSIX=y
 CONFIG_MODULES_COMMANDER=y
@@ -36,6 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -35,6 +35,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -36,6 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -35,6 +35,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -38,6 +38,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -39,6 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -40,6 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -41,6 +41,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -28,6 +28,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
@@ -15,6 +15,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
@@ -20,6 +20,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
@@ -1,3 +1,4 @@
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
 CONFIG_MODULES_MC_POS_CONTROL=n
@@ -43,6 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -44,6 +44,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -40,6 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -48,6 +48,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -53,7 +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_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -40,6 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
@@ -61,8 +61,8 @@ then
 	fi
 fi

-# Internal SPI bus ICM42686p (hard-mounted)
-icm42688p -6 -R 12 -b 1 -s start
+# Internal SPI bus ICM42688p (hard-mounted)
+icm42688p -R 12 -b 1 -s start

 # Internal on IMU SPI BMI088
 bmi088 -A -R 4 -s start
@@ -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_ICM42686P, 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_ICM42688P, 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, {
@@ -24,6 +24,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -12,7 +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_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_EVENTS=y
@@ -27,6 +27,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
@@ -31,6 +31,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_ROVER_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -22,7 +22,11 @@ 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 in hover), in rad [-PI..PI), NaN = leave to flight task
+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 loiter_radius		# loiter major axis radius in m
 float32 loiter_minor_radius	# loiter minor axis radius (used for non-circular loiter shapes) in m
@@ -35,3 +39,5 @@ 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
@@ -56,5 +56,4 @@ px4_add_module(
 		module.yaml
 	DEPENDS
 		git_gps_devices
-		drivers__device
 	)
@@ -45,11 +45,11 @@
 #include <poll.h>
 #endif

+#include <termios.h>
 #include <cstring>

 #include <drivers/drv_sensor.h>
 #include <lib/drivers/device/Device.hpp>
-#include <lib/drivers/device/Serial.hpp>
 #include <lib/parameters/param.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
@@ -81,7 +81,6 @@
 #include <linux/spi/spidev.h>
 #endif /* __PX4_LINUX */

-using namespace device;
 using namespace time_literals;

 #define TIMEOUT_1HZ		1300	//!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
@@ -170,10 +169,7 @@ public:
 	void reset_if_scheduled();

 private:
-#ifdef __PX4_LINUX
-	int				_spi_fd {-1};					///< SPI interface to GPS
-#endif
-	Serial 			*_uart = nullptr;	
+	int				_serial_fd{-1};					///< serial interface to GPS
 	unsigned			_baudrate{0};					///< current baudrate
 	const unsigned			_configured_baudrate{0};			///< configured baudrate (0=auto-detect)
 	char				_port[20] {};					///< device / serial port path
@@ -333,10 +329,8 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
 		set_device_bus(c - 48); // sub 48 to convert char to integer

-#ifdef __PX4_LINUX
 	} else if (_interface == GPSHelper::Interface::SPI) {
 		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
-#endif
 	}

 	if (_mode == gps_driver_mode_t::None) {
@@ -409,22 +403,10 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 			return num_read;
 		}

-	case GPSCallbackType::writeDeviceData: {
-			gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
+	case GPSCallbackType::writeDeviceData:
+		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);

-			int ret = 0;
-
-			if (gps->_uart) {
-				ret = gps->_uart->write((void *) data1, (size_t) data2);
-
-#ifdef __PX4_LINUX
-			} else if (gps->_spi_fd >= 0) {
-				ret = ::write(gps->_spi_fd, data1, (size_t)data2);
-#endif
-			}
-
-			return ret;
-		}
+		return ::write(gps->_serial_fd, data1, (size_t)data2);

 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -455,11 +437,10 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 			// as of 2021 setting the time on Nuttx temporarily pauses interrupts
 			// so only set the time if it is very wrong.
 			// TODO: clock slewing of the RTC for small time differences
-#ifndef __PX4_QURT
 			px4_clock_settime(CLOCK_REALTIME, &rtc_gps_time);
-#endif
 		}

+
 		break;
 	}

@@ -468,64 +449,72 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)

 int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 {
-	int ret = 0;
-	const unsigned character_count = 32; // minimum bytes that we want to read
-	const int max_timeout = 50;
-	int timeout_adjusted = math::min(max_timeout, timeout);
-
 	handleInjectDataTopic();

-	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-		ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
+#if !defined(__PX4_QURT)

-// SPI is only supported on Linux
-#if defined(__PX4_LINUX)
+	/* For non QURT, use the usual polling. */

-	} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
+	//Poll only for the serial data. In the same thread we also need to handle orb messages,
+	//so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
+	//two pollings use different underlying mechanisms (at least under posix), which makes this
+	//impossible. Instead we limit the maximum polling interval and regularly check for new orb
+	//messages.
+	//FIXME: add a unified poll() API
+	const int max_timeout = 50;

-		//Poll only for the SPI data. In the same thread we also need to handle orb messages,
-		//so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the
-		//two pollings use different underlying mechanisms (at least under posix), which makes this
-		//impossible. Instead we limit the maximum polling interval and regularly check for new orb
-		//messages.
-		//FIXME: add a unified poll() API
+	pollfd fds[1];
+	fds[0].fd = _serial_fd;
+	fds[0].events = POLLIN;

-		pollfd fds[1];
-		fds[0].fd = _spi_fd;
-		fds[0].events = POLLIN;
+	int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));

-		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
+	if (ret > 0) {
+		/* if we have new data from GPS, go handle it */
+		if (fds[0].revents & POLLIN) {
+			/*
+			 * We are here because poll says there is some data, so this
+			 * won't block even on a blocking device. But don't read immediately
+			 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+			 * If we have all requested data available, read it without waiting.
+			 * If more bytes are available, we'll go back to poll() again.
+			 */
+			const unsigned character_count = 32; // minimum bytes that we want to read
+			unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
+			const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);

-		if (ret > 0) {
-			/* if we have new data from GPS, go handle it */
-			if (fds[0].revents & POLLIN) {
-				/*
-				 * We are here because poll says there is some data, so this
-				 * won't block even on a blocking device. But don't read immediately
-				 * by 1-2 bytes, wait for some more data to save expensive read() calls.
-				 * If we have all requested data available, read it without waiting.
-				 * If more bytes are available, we'll go back to poll() again.
-				 */
-				unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
-				const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
+#ifdef __PX4_NUTTX
+			int err = 0;
+			int bytes_available = 0;
+			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);

+			if (err != 0 || bytes_available < (int)character_count) {
 				px4_usleep(sleeptime);
-
-				ret = ::read(_spi_fd, buf, buf_length);
-
-				if (ret > 0) {
-					_num_bytes_read += ret;
-				}
-
-			} else {
-				ret = -1;
 			}
-		}

+#else
+			px4_usleep(sleeptime);
 #endif
+
+			ret = ::read(_serial_fd, buf, buf_length);
+
+			if (ret > 0) {
+				_num_bytes_read += ret;
+			}
+
+		} else {
+			ret = -1;
+		}
 	}

 	return ret;
+
+#else
+	/* For QURT, just use read for now, since this doesn't block, we need to slow it down
+	 * just a bit. */
+	px4_usleep(10000);
+	return ::read(_serial_fd, buf, buf_length);
+#endif
 }

 void GPS::handleInjectDataTopic()
@@ -594,38 +583,105 @@ bool GPS::injectData(uint8_t *data, size_t len)
 {
 	dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);

-	size_t written = 0;
-
-	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-		written = _uart->write((const void *) data, len);
-
-#ifdef __PX4_LINUX
-
-	} else if (_interface == GPSHelper::Interface::SPI) {
-		written = ::write(_spi_fd, data, len);
-		::fsync(_spi_fd);
-#endif
-	}
-
+	size_t written = ::write(_serial_fd, data, len);
+	::fsync(_serial_fd);
 	return written == len;
 }

 int GPS::setBaudrate(unsigned baud)
 {
-	if (_interface == GPSHelper::Interface::UART) {
-		if ((_uart) && (_uart->setBaudrate(baud))) {
-			return 0;
-		}
+	/* process baud rate */
+	int speed;

-#ifdef __PX4_LINUX
+	switch (baud) {
+	case 9600:   speed = B9600;   break;

-	} else if (_interface == GPSHelper::Interface::SPI) {
-		// Can't set the baudrate on a SPI port but just return a success
-		return 0;
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
 #endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %d", baud);
+		return -EINVAL;
 	}

-	return -1;
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(_serial_fd, &uart_config);
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return -1;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return -1;
+	}
+
+	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
+		GPS_ERR("ERR: %d (tcsetattr)", termios_state);
+		return -1;
+	}
+
+	return 0;
 }

 void GPS::initializeCommunicationDump()
@@ -784,58 +840,31 @@ GPS::run()
 			_helper = nullptr;
 		}

-		if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
+		if (_serial_fd < 0) {
+			/* open the serial port */
+			_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);

-			// Create the UART port instance
-			_uart = new Serial(_port);
-
-			if (_uart == nullptr) {
-				PX4_ERR("Error creating serial device %s", _port);
-				px4_usleep(100000);
-				continue;
-			}
-		}
-
-		if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) {
-			// Configure the desired baudrate if one was specified by the user.
-			// Otherwise the default baudrate will be used.
-			if (_configured_baudrate) {
-				if (! _uart->setBaudrate(_configured_baudrate)) {
-					PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
-					px4_usleep(100000);
-					continue;
-				}
-			}
-
-			// Open the UART. If this is successful then the UART is ready to use.
-			if (! _uart->open()) {
-				PX4_ERR("Error opening serial device  %s", _port);
-				px4_usleep(100000);
+			if (_serial_fd < 0) {
+				PX4_ERR("failed to open %s err: %d", _port, errno);
+				px4_sleep(1);
 				continue;
 			}

 #ifdef __PX4_LINUX

-		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
-			_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
+			if (_interface == GPSHelper::Interface::SPI) {
+				int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
+				int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);

-			if (_spi_fd < 0) {
-				PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
-				px4_sleep(1);
-				continue;
-			}
+				if (status_value < 0) {
+					PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				}

-			int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
-			int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+				status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);

-			if (status_value < 0) {
-				PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-			}
-
-			status_value = ::ioctl(_spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
-
-			if (status_value < 0) {
-				PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				if (status_value < 0) {
+					PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				}
 			}

 #endif /* __PX4_LINUX */
@@ -1027,17 +1056,9 @@ GPS::run()
 			}
 		}

-		if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-			(void) _uart->close();
-			delete _uart;
-			_uart = nullptr;
-
-#ifdef __PX4_LINUX
-
-		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
-			::close(_spi_fd);
-			_spi_fd = -1;
-#endif
+		if (_serial_fd >= 0) {
+			::close(_serial_fd);
+			_serial_fd = -1;
 		}

 		if (_mode_auto) {
@@ -1456,12 +1477,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;

 		case 'i':
-			if (!strcmp(myoptarg, "uart")) {
-				interface = GPSHelper::Interface::UART;
-#ifdef __PX4_LINUX
-			} else if (!strcmp(myoptarg, "spi")) {
+			if (!strcmp(myoptarg, "spi")) {
 				interface = GPSHelper::Interface::SPI;
-#endif
+
+			} else if (!strcmp(myoptarg, "uart")) {
+				interface = GPSHelper::Interface::UART;
+
 			} else {
 				PX4_ERR("unknown interface: %s", myoptarg);
 				error_flag = true;
@@ -1469,12 +1490,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;

 		case 'j':
-			if (!strcmp(myoptarg, "uart")) {
-				interface_secondary = GPSHelper::Interface::UART;
-#ifdef __PX4_LINUX
-			} else if (!strcmp(myoptarg, "spi")) {
+			if (!strcmp(myoptarg, "spi")) {
 				interface_secondary = GPSHelper::Interface::SPI;
-#endif
+
+			} else if (!strcmp(myoptarg, "uart")) {
+				interface_secondary = GPSHelper::Interface::UART;
+
 			} else {
 				PX4_ERR("unknown interface for secondary: %s", myoptarg);
 				error_flag = true;
@@ -126,9 +126,8 @@ 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 == expected_whoami) {
+		if (whoami == WHOAMI || (isICM686 && whoami == WHOAMI686)) {
 			return PX4_OK;

 		} else {
@@ -53,6 +53,7 @@
 #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
@@ -69,6 +69,21 @@ 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
 *
@@ -40,31 +40,20 @@ if (${PX4_PLATFORM} STREQUAL "nuttx")
 	if ("${CONFIG_SPI}" STREQUAL "y")
 		list(APPEND SRCS_PLATFORM nuttx/SPI.cpp)
 	endif()
-
-	list(APPEND SRCS_PLATFORM nuttx/SerialImpl.cpp)
 elseif((${PX4_PLATFORM} MATCHES "qurt"))
 	list(APPEND SRCS_PLATFORM qurt/I2C.cpp)
 	list(APPEND SRCS_PLATFORM qurt/SPI.cpp)
-	list(APPEND SRCS_PLATFORM qurt/SerialImpl.cpp)
 	list(APPEND SRCS_PLATFORM qurt/uart.c)
 elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type
 	# Linux I2Cdev and SPIdev
-	if ("${CONFIG_I2C}" STREQUAL "y")
-		list(APPEND SRCS_PLATFORM posix/I2C.cpp)
-	endif()
-
-	if ("${CONFIG_SPI}" STREQUAL "y")
-		list(APPEND SRCS_PLATFORM posix/SPI.cpp)
-	endif()
-
-	list(APPEND SRCS_PLATFORM posix/SerialImpl.cpp)
-	list(APPEND SRCS_PLATFORM posix/SerialSBUSImpl.cpp)
-	list(APPEND SRCS_PLATFORM posix/SerialStandardImpl.cpp)
+	list(APPEND SRCS_PLATFORM
+		posix/I2C.cpp
+		posix/SPI.cpp
+	)
 endif()

 px4_add_library(drivers__device
 	CDev.cpp
-	Serial.cpp
 	${SRCS_PLATFORM}
 	)

@@ -1,89 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-// Bring in the correct platform implementation
-#ifdef __PX4_NUTTX
-#include "nuttx/SerialImpl.hpp"
-#elif defined(__PX4_QURT)
-#include "qurt/SerialImpl.hpp"
-#else
-#include "posix/SerialImpl.hpp"
-#endif
-
-namespace device __EXPORT
-{
-
-class Serial
-{
-public:
-	// Baud rate can be selected with constructor or by using setBaudrate
-	Serial(const char *port, uint32_t baudrate = 0);
-	virtual ~Serial();
-
-	// Open sets up the port and gets it configured. Unless an alternate mode
-	// is selected the port will be configured with parity disabled and 1 stop bit.
-	bool open();
-	bool isOpen() const;
-
-	bool close();
-
-	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
-
-	ssize_t write(const void *buffer, size_t buffer_size);
-
-	uint32_t getBaudrate() const;
-
-	// If the port has already been opened it will be reconfigured with a change
-	// of baudrate.
-	bool setBaudrate(uint32_t baudrate);
-
-	// SBUS has special configuration considerations and methods so it
-	// is given a special mode. It has parity enabled and 2 stop bits
-	bool getSBUSMode() const;
-	bool setSBUSMode(bool enable);
-
-	const char *getPort() const;
-
-private:
-	// Disable copy constructors
-	Serial(const Serial &);
-	Serial &operator=(const Serial &);
-
-	// platform implementation
-	SerialImpl _impl;
-};
-
-} // namespace device
@@ -1,363 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SerialImpl.hpp"
-#include <string.h> // strncpy
-#include <termios.h>
-#include <px4_log.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <poll.h>
-#include <drivers/drv_hrt.h>
-#include <board_config.h>
-
-namespace device
-{
-
-SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-
- 	if (baudrate) {
-		_baudrate = baudrate;
-	} else {
-		// If baudrate is zero then choose a reasonable default
-		_baudrate = 9600;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	if (isOpen()) {
-		close();
-	}
-}
-
-bool SerialImpl::configure()
-{
-	struct termios uart_config;
-
-	if (_SBUSMode) {
-		if (_baudrate != DEFAULT_SBUS_BAUDRATE) {
-			PX4_WARN("Warning, SBUS baud rate being set to %lu", _baudrate);
-		}
-
-		/* even parity, two stop bits */
-		tcgetattr(_serial_fd, &uart_config);
-		cfsetspeed(&uart_config, _baudrate);
-		uart_config.c_cflag |= (CSTOPB | PARENB);
-		tcsetattr(_serial_fd, TCSANOW, &uart_config);
-
-		if (board_rc_singlewire(_port)) {
-			/* only defined in configs capable of IOCTL
-			 * Note It is never turned off
-			 */
-#ifdef TIOCSSINGLEWIRE
-			ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
-#endif
-		}
-	} else {
-		/* process baud rate */
-		int speed;
-
-		switch (_baudrate) {
-		case 9600:   speed = B9600;   break;
-
-		case 19200:  speed = B19200;  break;
-
-		case 38400:  speed = B38400;  break;
-
-		case 57600:  speed = B57600;  break;
-
-		case 115200: speed = B115200; break;
-
-		case 230400: speed = B230400; break;
-
-#ifndef B460800
-#define B460800 460800
-#endif
-
-		case 460800: speed = B460800; break;
-
-#ifndef B921600
-#define B921600 921600
-#endif
-
-		case 921600: speed = B921600; break;
-
-		default:
-			PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
-			return false;
-		}
-
-		int termios_state;
-
-		/* fill the struct for the new configuration */
-		if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
-			PX4_ERR("ERR: %d (tcgetattr)", termios_state);
-			return false;
-		}
-
-		/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
-
-		//
-		// Input flags - Turn off input processing
-		//
-		// convert break to null byte, no CR to NL translation,
-		// no NL to CR translation, don't mark parity errors or breaks
-		// no input parity check, don't strip high bit off,
-		// no XON/XOFF software flow control
-		//
-		uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
-					 INLCR | PARMRK | INPCK | ISTRIP | IXON);
-
-		//
-		// Output flags - Turn off output processing
-		//
-		// no CR to NL translation, no NL to CR-NL translation,
-		// no NL to CR translation, no column 0 CR suppression,
-		// no Ctrl-D suppression, no fill characters, no case mapping,
-		// no local output processing
-		//
-		// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
-		//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
-		uart_config.c_oflag = 0;
-
-		//
-		// No line processing
-		//
-		// echo off, echo newline off, canonical mode off,
-		// extended input processing off, signal chars off
-		//
-		uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-
-		/* no parity, one stop bit, disable flow control */
-		uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
-
-		/* set baud rate */
-		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-			PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
-			return false;
-		}
-
-		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-			PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
-			return false;
-		}
-
-		if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
-			PX4_ERR("ERR: %d (tcsetattr)", termios_state);
-			return false;
-		}
-	}
-
-	return true;
-}
-
-bool SerialImpl::open()
-{
-	if (isOpen()) {
-		return true;
-	}
-
-	// Open the serial port
-	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
-
-	if (serial_fd < 0) {
-		PX4_ERR("failed to open %s err: %d", _port, errno);
-		return false;
-	}
-
-	_serial_fd = serial_fd;
-
-	// Configure the serial port
-	if (! configure()) {
-		PX4_ERR("failed to configure %s err: %d", _port, errno);
-		close();
-		return false;
-	}
-
-	_open = true;
-
-	return _open;
-}
-
-bool SerialImpl::isOpen() const
-{
-	return _open;
-}
-
-bool SerialImpl::close()
-{
-	if (_serial_fd >= 0) {
-		::close(_serial_fd);
-	}
-
-	_serial_fd = -1;
-	_open = false;
-
-	return true;
-}
-
-ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot read from serial device until it has been opened");
-		return -1;
-	}
-
-	int ret = ::read(_serial_fd, buffer, buffer_size);
-
-	if (ret < 0) {
-		PX4_DEBUG("%s read error %d", _port, ret);
-	}
-
-	return ret;
-}
-
-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
-{
-	if (!_open) {
-		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
-		return -1;
-	}
-
-	if (buffer_size < character_count) {
-		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
-		return -1;
-	}
-
-	const hrt_abstime start_time_us = hrt_absolute_time();
-	int total_bytes_read = 0;
-
-	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
-		// Poll for incoming UART data.
-		pollfd fds[1];
-		fds[0].fd = _serial_fd;
-		fds[0].events = POLLIN;
-
-		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
-
-		if (remaining_time <= 0) { break; }
-
-		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
-
-		if (ret > 0) {
-			if (fds[0].revents & POLLIN) {
-				ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
-
-				if (ret > 0) {
-					total_bytes_read += ret;
-				}
-
-			} else {
-				PX4_ERR("Got a poll error");
-				return -1;
-			}
-		}
-	}
-
-	return total_bytes_read;
-}
-
-ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot write to serial device until it has been opened");
-		return -1;
-	}
-
-	int written = ::write(_serial_fd, buffer, buffer_size);
-	::fsync(_serial_fd);
-
-	if (written < 0) {
-		PX4_ERR("%s write error %d", _port, written);
-	}
-
-	return written;
-}
-
-const char *SerialImpl::getPort() const
-{
-	return _port;
-}
-
-uint32_t SerialImpl::getBaudrate() const
-{
-	return _baudrate;
-}
-
-bool SerialImpl::setBaudrate(uint32_t baudrate)
-{
-	// check if already configured
-	if (baudrate == _baudrate) {
-		return true;
-	}
-
-	_baudrate = baudrate;
-
-	// process baud rate change now if port is already open
-	if ((_open) && (configure() != 0)) {
-		// Configure failed! Close the port
-		close();
-		return false;
-	}
-
-	return true;
-}
-
-bool SerialImpl::getSBUSMode() const
-{
-	return _SBUSMode;
-}
-
-bool SerialImpl::setSBUSMode(bool enable)
-{
-	if (_open) {
-		PX4_ERR("Cannot configure SBUS mode after port has already been opened");
-		return false;
-	}
-
-	_SBUSMode = enable;
-	_baudrate = DEFAULT_SBUS_BAUDRATE;
-
-	return true;
-}
-
-} // namespace device
@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <stdint.h>
-
-namespace device
-{
-
-class SerialImpl
-{
-public:
-
-	SerialImpl(const char *port, uint32_t baudrate);
-	virtual ~SerialImpl();
-
-	bool open();
-	bool isOpen() const;
-
-	bool close();
-
-	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
-
-	ssize_t write(const void *buffer, size_t buffer_size);
-
-	const char *getPort() const;
-
-	uint32_t getBaudrate() const;
-	bool setBaudrate(uint32_t baudrate);
-
-	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
@@ -1,253 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SerialImpl.hpp"
-
-#include <unistd.h>
-
-#include <string.h> // strncpy
-#include <px4_log.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <poll.h>
-#include <drivers/drv_hrt.h>
-
-namespace device
-{
-
-SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-
- 	if (baudrate) {
-		_baudrate = baudrate;
-	} else {
-		// If baudrate is zero then choose a reasonable default
-		_baudrate = 9600;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	if (isOpen()) {
-		close();
-	}
-}
-
-bool SerialImpl::configure()
-{
-	if (_SBUSMode) {
-		return _sbus.configure(_serial_fd, _baudrate);
-	}
-
-	return _standard.configure(_serial_fd, _baudrate);
-}
-
-bool SerialImpl::open()
-{
-	if (isOpen()) {
-		return true;
-	}
-
-	// Open the serial port
-	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
-
-	if (serial_fd < 0) {
-		PX4_ERR("failed to open %s err: %d", _port, errno);
-		return false;
-	}
-
-	_serial_fd = serial_fd;
-
-	// Configure the serial port
-	if (! configure()) {
-		PX4_ERR("failed to configure %s err: %d", _port, errno);
-		close();
-		return false;
-	}
-
-	_open = true;
-
-	return _open;
-}
-
-bool SerialImpl::isOpen() const
-{
-	return _open;
-}
-
-bool SerialImpl::close()
-{
-	if (_serial_fd >= 0) {
-		::close(_serial_fd);
-	}
-
-	_serial_fd = -1;
-	_open = false;
-
-	return true;
-}
-
-ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot read from serial device until it has been opened");
-		return -1;
-	}
-
-	int ret = ::read(_serial_fd, buffer, buffer_size);
-
-	if (ret < 0) {
-		PX4_DEBUG("%s read error %d", _port, ret);
-	}
-
-	return ret;
-}
-
-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
-{
-	if (!_open) {
-		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
-		return -1;
-	}
-
-	if (buffer_size < character_count) {
-		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
-		return -1;
-	}
-
-	const hrt_abstime start_time_us = hrt_absolute_time();
-	int total_bytes_read = 0;
-
-	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
-		// Poll for incoming UART data.
-		pollfd fds[1];
-		fds[0].fd = _serial_fd;
-		fds[0].events = POLLIN;
-
-		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
-
-		if (remaining_time <= 0) { break; }
-
-		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
-
-		if (ret > 0) {
-			if (fds[0].revents & POLLIN) {
-				ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
-
-				if (ret > 0) {
-					total_bytes_read += ret;
-				}
-
-			} else {
-				PX4_ERR("Got a poll error");
-				return -1;
-			}
-		}
-	}
-
-	return total_bytes_read;
-}
-
-ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot write to serial device until it has been opened");
-		return -1;
-	}
-
-	int written = ::write(_serial_fd, buffer, buffer_size);
-	::fsync(_serial_fd);
-
-	if (written < 0) {
-		PX4_ERR("%s write error %d", _port, written);
-
-	}
-
-	return written;
-}
-
-const char *SerialImpl::getPort() const
-{
-	return _port;
-}
-
-uint32_t SerialImpl::getBaudrate() const
-{
-	return _baudrate;
-}
-
-bool SerialImpl::setBaudrate(uint32_t baudrate)
-{
-	// check if already configured
-	if (baudrate == _baudrate) {
-		return true;
-	}
-
-	_baudrate = baudrate;
-
-	// process baud rate change now if port is already open
-	if ((_open) && (configure() != 0)) {
-		// Configure failed! Close the port
-		close();
-		return false;
-	}
-
-	return true;
-}
-
-bool SerialImpl::getSBUSMode() const
-{
-	return _SBUSMode;
-}
-
-bool SerialImpl::setSBUSMode(bool enable)
-{
-	if (_open) {
-		PX4_ERR("Cannot configure SBUS mode after port has already been opened");
-		return false;
-	}
-
-	_SBUSMode = enable;
-	_baudrate = SerialSBUSImpl::DEFAULT_BAUDRATE;
-
-	return true;
-}
-
-} // namespace device
@@ -1,89 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <stdint.h>
-
-#include "SerialSBUSImpl.hpp"
-#include "SerialStandardImpl.hpp"
-
-namespace device
-{
-
-class SerialImpl
-{
-public:
-
-	SerialImpl(const char *port, uint32_t baudrate);
-	virtual ~SerialImpl();
-
-	bool open();
-	bool isOpen() const;
-
-	bool close();
-
-	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
-
-	ssize_t write(const void *buffer, size_t buffer_size);
-
-	const char *getPort() const;
-
-	uint32_t getBaudrate() const;
-	bool setBaudrate(uint32_t baudrate);
-
-	bool getSBUSMode() const;
-	bool setSBUSMode(bool enable);
-
-private:
-
-	int _serial_fd{-1};
-
-	bool _open{false};
-
-	char _port[32] {};
-
-	uint32_t _baudrate{0};
-
-	bool _SBUSMode{false};
-
-	// The configuration routines for SBUS versus other needed to be separated
-	// out because they use different methods with different, conflicting header files
-	SerialSBUSImpl _sbus;
-	SerialStandardImpl _standard;
-
-	bool configure();
-};
-
-} // namespace device
@@ -1,80 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SerialSBUSImpl.hpp"
-
-#include <sys/ioctl.h>
-#include <asm-generic/termbits.h>
-#include <px4_log.h>
-
-namespace device
-{
-
-bool SerialSBUSImpl::configure(int fd, uint32_t baud)
-{
-	struct termios2 tio = {};
-
-	if (ioctl(fd, TCGETS2, &tio)) {
-		return false;
-	}
-
-	if (baud != DEFAULT_BAUDRATE) {
-		PX4_WARN("Warning, SBUS baud rate being set to %u", baud);
-	}
-
-	/**
-	 * Setting serial port,8E2, non-blocking.100Kbps
-	 */
-	tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL
-			 | IXON);
-	tio.c_iflag |= (INPCK | IGNPAR);
-	tio.c_oflag &= ~OPOST;
-	tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
-	tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
-	/**
-	 * use BOTHER to specify speed directly in c_[io]speed member
-	 */
-	tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);
-	tio.c_ispeed = baud;
-	tio.c_ospeed = baud;
-	tio.c_cc[VMIN] = 25;
-	tio.c_cc[VTIME] = 0;
-
-	if (ioctl(fd, TCSETS2, &tio)) {
-		return false;
-	}
-
-	return true;
-}
-
-} // namespace device
@@ -1,53 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <stdint.h>
-
-namespace device
-{
-
-class SerialSBUSImpl
-{
-public:
-
-	SerialSBUSImpl() {};
-	virtual ~SerialSBUSImpl() {};
-
-	bool configure(int fd, uint32_t baud);
-
-	static const uint32_t DEFAULT_BAUDRATE{100000};
-};
-
-} // namespace device
@@ -1,159 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SerialStandardImpl.hpp"
-
-#include <sys/termios.h>
-#include <px4_log.h>
-
-namespace device
-{
-
-bool SerialStandardImpl::validateBaudrate(uint32_t baudrate)
-{
-	return ((baudrate == 9600) ||
-		(baudrate == 19200) ||
-		(baudrate == 38400) ||
-		(baudrate == 57600) ||
-		(baudrate == 115200) ||
-		(baudrate == 230400) ||
-		(baudrate == 460800) ||
-		(baudrate == 921600));
-}
-
-bool SerialStandardImpl::configure(int fd, uint32_t baud)
-{
-	/* process baud rate */
-	int speed;
-
-	if (! validateBaudrate(baud)) {
-		PX4_ERR("ERR: unknown baudrate: %u", baud);
-		return false;
-	}
-
-	switch (baud) {
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-#ifndef B460800
-#define B460800 460800
-#endif
-
-	case 460800: speed = B460800; break;
-
-#ifndef B921600
-#define B921600 921600
-#endif
-
-	case 921600: speed = B921600; break;
-
-	default:
-		PX4_ERR("ERR: unknown baudrate: %d", baud);
-		return false;
-	}
-
-	struct termios uart_config;
-
-	int termios_state;
-
-	/* fill the struct for the new configuration */
-	if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
-		return false;
-	}
-
-	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
-
-	//
-	// Input flags - Turn off input processing
-	//
-	// convert break to null byte, no CR to NL translation,
-	// no NL to CR translation, don't mark parity errors or breaks
-	// no input parity check, don't strip high bit off,
-	// no XON/XOFF software flow control
-	//
-	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
-				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
-
-	//
-	// Output flags - Turn off output processing
-	//
-	// no CR to NL translation, no NL to CR-NL translation,
-	// no NL to CR translation, no column 0 CR suppression,
-	// no Ctrl-D suppression, no fill characters, no case mapping,
-	// no local output processing
-	//
-	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
-	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
-	uart_config.c_oflag = 0;
-
-	//
-	// No line processing
-	//
-	// echo off, echo newline off, canonical mode off,
-	// extended input processing off, signal chars off
-	//
-	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-
-	/* no parity, one stop bit, disable flow control */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
-
-	/* set baud rate */
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
-		return false;
-	}
-
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
-		return false;
-	}
-
-	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
-		return false;
-	}
-
-	return true;
-}
-
-} // namespace device
@@ -1,55 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <stdint.h>
-
-namespace device
-{
-
-class SerialStandardImpl
-{
-public:
-
-	SerialStandardImpl() {};
-	virtual ~SerialStandardImpl() {};
-
-	bool configure(int fd, uint32_t baud);
-
-private:
-	bool validateBaudrate(uint32_t baudrate);
-
-};
-
-} // namespace device
@@ -1,239 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SerialImpl.hpp"
-#include <string.h> // strncpy
-#include <px4_log.h>
-#include <drivers/device/qurt/uart.h>
-#include <drivers/drv_hrt.h>
-
-namespace device
-{
-
-SerialImpl::SerialImpl(const char *port, uint32_t baudrate)
-{
-	if (port) {
-		strncpy(_port, port, sizeof(_port) - 1);
-		_port[sizeof(_port) - 1] = '\0';
-
-	} else {
-		_port[0] = 0;
-	}
-
- 	if (baudrate) {
-		_baudrate = baudrate;
-	} else {
-		// If baudrate is zero then choose a reasonable default.
-		// The default is the GPS UBX M10 default rate.
-		_baudrate = 115200;
-	}
-}
-
-SerialImpl::~SerialImpl()
-{
-	if (isOpen()) {
-		close();
-	}
-}
-
-bool SerialImpl::open()
-{
-	// There's no harm in calling open multiple times on the same port.
-	// In fact, that's the only way to change the baudrate
-
-	_open = false;
-	_serial_fd = -1;
-
-	// qurt_uart_open will check validity of port and baudrate
-	int serial_fd = qurt_uart_open(_port, _baudrate);
-
-	if (serial_fd < 0) {
-		PX4_ERR("failed to open %s at baudrate %u, fd: %d", _port, _baudrate, serial_fd);
-		close();
-		return false;
-
-	} else {
-		PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate);
-	}
-
-	_serial_fd = serial_fd;
-	_open = true;
-
-	return _open;
-}
-
-bool SerialImpl::isOpen() const
-{
-	return _open;
-}
-
-bool SerialImpl::close()
-{
-	// No close defined for qurt uart yet
-	// if (_serial_fd >= 0) {
-	// 	qurt_uart_close(_serial_fd);
-	// }
-
-	_serial_fd = -1;
-	_open = false;
-
-	return true;
-}
-
-ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot read from serial device until it has been opened");
-		return -1;
-	}
-
-	int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500);
-
-	if (ret_read < 0) {
-		PX4_DEBUG("%s read error %d", _port, ret_read);
-
-	}
-
-	return ret_read;
-}
-
-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
-{
-	if (!_open) {
-		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
-		return -1;
-	}
-
-	if (buffer_size < character_count) {
-		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
-		return -1;
-	}
-
-	const hrt_abstime start_time_us = hrt_absolute_time();
-	int total_bytes_read = 0;
-
-	while (total_bytes_read < (int) character_count) {
-
-		if (timeout_us > 0) {
-			const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
-
-			if (elapsed_us >= timeout_us) {
-				// If there was a partial read but not enough to satisfy the minimum then they will be lost
-				// but this really should never happen when everything is working normally.
-				// PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us);
-				// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
-				return total_bytes_read;
-			}
-		}
-
-		int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
-
-		if (current_bytes_read < 0) {
-			// Again, if there was a partial read but not enough to satisfy the minimum then they will be lost
-			// but this really should never happen when everything is working normally.
-			PX4_ERR("%s failed to read uart", __FUNCTION__);
-			// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
-			return -1;
-		}
-
-		// Current bytes read could be zero
-		total_bytes_read += current_bytes_read;
-
-		// If we have at least reached our desired minimum number of characters
-		// then we can return now
-		if (total_bytes_read >= (int) character_count) {
-			return total_bytes_read;
-		}
-
-		// Wait a set amount of time before trying again or the remaining time
-		// until the timeout if we are getting close
-		const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
-		int64_t time_until_timeout = timeout_us - elapsed_us;
-		uint64_t time_to_sleep = 5000;
-
-		if ((time_until_timeout >= 0) &&
-		    (time_until_timeout < (int64_t) time_to_sleep)) {
-			time_to_sleep = time_until_timeout;
-		}
-
-		px4_usleep(time_to_sleep);
-	}
-
-	return -1;
-}
-
-ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
-{
-	if (!_open) {
-		PX4_ERR("Cannot write to serial device until it has been opened");
-		return -1;
-	}
-
-	int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size);
-
-	if (ret_write < 0) {
-		PX4_ERR("%s write error %d", _port, ret_write);
-
-	}
-
-	return ret_write;
-}
-
-const char *SerialImpl::getPort() const
-{
-	return _port;
-}
-
-uint32_t SerialImpl::getBaudrate() const
-{
-	return _baudrate;
-}
-
-bool SerialImpl::setBaudrate(uint32_t baudrate)
-{
-	// check if already configured
-	if (baudrate == _baudrate) {
-		return true;
-	}
-
-	_baudrate = baudrate;
-
-	// process baud rate change now if port is already open
-	if (_open) {
-		return open();
-	}
-
-	return true;
-}
-
-} // namespace device
@@ -1,80 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <stdint.h>
-
-namespace device
-{
-
-class SerialImpl
-{
-public:
-
-	SerialImpl(const char *port, uint32_t baudrate);
-	virtual ~SerialImpl();
-
-	bool open();
-	bool isOpen() const;
-
-	bool close();
-
-	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
-
-	ssize_t write(const void *buffer, size_t buffer_size);
-
-	const char *getPort() const;
-
-	uint32_t getBaudrate() const;
-	bool setBaudrate(uint32_t baudrate);
-
-	// Cannot configure Qurt UARTs for SBUS!
-	bool getSBUSMode() const { return false; }
-	bool setSBUSMode(bool enable) { return false; }
-
-private:
-
-	int _serial_fd{-1};
-
-	bool _open{false};
-
-	char _port[32] {};
-
-	uint32_t _baudrate{0};
-
-	bool validateBaudrate(uint32_t baudrate);
-};
-
-} // namespace device
@@ -35,8 +35,6 @@ 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);
@@ -54,12 +52,6 @@ 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)
 	{
@@ -262,12 +262,3 @@ 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));
-}
@@ -85,7 +85,7 @@ public:

 	float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const;
 	/*
-	 * Computes the lateral acceleration and true airspeed references necessary to track
+	 * Computes the lateral acceleration and 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 (true airspeed) [m/s].
+	 * Set the nominal airspeed reference [m/s].
 	 */
 	void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); }

 	/*
-	 * Set the maximum airspeed reference (true airspeed) [m/s].
+	 * Set the maximum airspeed reference [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) 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 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 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}; // true airspeed reference [m/s]
-	matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // true air velocity reference vector [m/s]
+	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 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 true airspeed [m/s]
+	 * @param[in] airspeed Vehicle 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 true airspeed [m/s]
+	 * @param[in] airspeed Vehicle 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 true airspeed, in a no-wind condition [rad/s]
+	 *            curvature at the current 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 true airspeed, in a no-wind condition [rad/s]
+	 *            curvature at the current 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" true airspeed reference compensation in excess wind conditions.
-	 * Nominal and maximum true airspeed member variables must be set before using this method.
+	 * including "optimal" airspeed reference compensation in excess wind conditions.
+	 * Nominal and maximum 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 true airspeed [m/s]
+	 * @param[in] airspeed Vehicle 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 true airspeed [m/s]
+	 * @param[in] airspeed Vehicle 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 true airspeed [m/s]
+	 * @param[in] airspeed Vehicle 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 true airspeed [m/s]
+	 * @param[in] airspeed Vehicle 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 true airspeed [m/s]
+	 * @param[in] airspeed Vehicle 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 true airspeed [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
 	 * @return Lateral acceleration demand [m/s^2]
 	 */
 	float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,
@@ -42,36 +42,7 @@

 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;
 }
@@ -216,19 +216,6 @@ 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
 *
@@ -600,8 +600,9 @@ void AirspeedModule::select_airspeed_and_publish()
 	}

 	// check if airspeed based on ground-wind speed is valid and can be published
-	if (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
-	    || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX) {
+	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)) {

 		// _vehicle_local_position_valid determines if ground-wind estimate is valid
 		if (_vehicle_local_position_valid &&
@@ -132,6 +132,7 @@ 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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -37,20 +37,24 @@
 using namespace time_literals;

 AirspeedChecks::AirspeedChecks()
-	: _param_fw_airspd_max_handle(param_find("FW_AIRSPD_MAX"))
+	: _param_fw_arsp_mode_handle(param_find("FW_ARSP_MODE")), _param_fw_airspd_max_handle(param_find("FW_AIRSPD_MAX"))
 {
 }

 void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (_param_sys_has_num_aspd.get() <= 0 ||
+	if (circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(), CBRK_AIRSPD_CHK_KEY) ||
 	    (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) < 2_s) {
+	if (_airspeed_validated_sub.copy(&airspeed_validated) && hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s) {

 		reporter.setIsPresent(health_component_t::differential_pressure);

@@ -96,13 +100,13 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
 			}
 		}

-	} else {
+	} else if (!optional) {

 		/* EVENT
 		 * @description
 		 * <profile name="dev">
 		 * Most likely the airspeed selector module is not running.
-		 * This check can be configured via <param>SYS_HAS_NUM_ASPD</param> parameter.
+		 * This check can be configured via <param>CBRK_AIRSPD_CHK</param> parameter.
 		 * </profile>
 		 */
 		reporter.healthFailure(NavModes::All, health_component_t::differential_pressure,
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -49,9 +49,10 @@ 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::SYS_HAS_NUM_ASPD>) _param_sys_has_num_aspd
+					(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk
 				       )
 };
@@ -239,7 +239,7 @@ ControlAllocator::update_effectiveness_source()
 			break;

 		case EffectivenessSource::ROVER_DIFFERENTIAL:
-			// differential_drive_control does allocation and publishes directly to actuator_motors topic
+			// rover_control does allocation and publishes directly to actuator_motors topic
 			break;

 		case EffectivenessSource::FIXED_WING:
@@ -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.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
+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
 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.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
+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
 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
@@ -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.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
+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
 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
@@ -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(PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw));
+	_weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane);

 	// 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,
-				(float)NAN,
+				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
 				_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(
 			_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
@@ -509,7 +509,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yaw_setpoint = NAN;
 			_yawspeed_setpoint = 0.f;

-		} else if (PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)) {
+		} 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
 			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
 			_yawspeed_setpoint = NAN;

@@ -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_use_airspd.get() && airspeed_valid) {
+	if ((_param_fw_arsp_mode.get() == 0) && 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);

@@ -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,
-		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
+		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,

 		(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
 		(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
@@ -75,7 +75,6 @@ bool FwAutotuneAttitudeControl::init()

 void FwAutotuneAttitudeControl::reset()
 {
-	_param_fw_at_start.reset();
 }

 void FwAutotuneAttitudeControl::Run()
@@ -59,6 +59,9 @@ 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);
@@ -77,6 +80,7 @@ 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);

@@ -107,6 +111,11 @@ 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());
@@ -205,23 +214,20 @@ FixedwingPositionControl::airspeed_poll()
 	bool airspeed_valid = _airspeed_valid;
 	airspeed_validated_s airspeed_validated;

-	if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {
+	if ((_param_fw_arsp_mode.get() == 0) && _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 > FLT_EPSILON)) {
+		    && (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) {

 			airspeed_valid = true;

 			_time_airspeed_last_valid = airspeed_validated.timestamp;
-			_airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
+			_airspeed = 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 {
@@ -385,15 +391,8 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 	calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
 				       _performance_model.getMaximumCalibratedAirspeed());

-	// 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 (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) {
+		_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
 	}

 	if (control_interval > FLT_EPSILON) {
@@ -401,6 +400,10 @@ 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());
 	}
@@ -645,7 +648,7 @@ void
 FixedwingPositionControl::updateManualTakeoffStatus()
 {
 	if (!_completed_manual_takeoff) {
-		const bool at_controllable_airspeed = _airspeed_eas > _performance_model.getMinimumCalibratedAirspeed(getLoadFactor())
+		const bool at_controllable_airspeed = _airspeed > _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;
@@ -1369,7 +1372,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_runway_takeoff.forceSetFlyState();
 		}

-		_runway_takeoff.update(now, takeoff_airspeed, _airspeed_eas, _current_altitude - _takeoff_ground_alt,
+		_runway_takeoff.update(now, takeoff_airspeed, _airspeed, _current_altitude - _takeoff_ground_alt,
 				       clearance_altitude_amsl - _takeoff_ground_alt);

 		// yaw control is disabled once in "taking off" state
@@ -2513,14 +2516,48 @@ 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)
 {
-	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
-	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;
+	_tecs_is_running = true;

-	} else {
-		_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) {
+		return;
 	}

 	/* update TECS vehicle state estimates */
@@ -2538,7 +2575,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     _current_altitude,
 		     alt_sp,
 		     airspeed_sp,
-		     _airspeed_eas,
+		     _airspeed,
 		     _eas2tas,
 		     throttle_min,
 		     throttle_max,
@@ -360,8 +360,8 @@ private:

 	// AIRSPEED

-	float _airspeed_eas{0.f};
-	float _eas2tas{1.f};
+	float _airspeed{0.0f};
+	float _eas2tas{1.0f};
 	bool _airspeed_valid{false};
 	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};

@@ -384,8 +384,13 @@ 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

@@ -954,7 +959,7 @@ private:
 		(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,

 		// external parameters
-		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
+		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,

 		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,

@@ -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_use_airspd.get() && airspeed_valid) {
+	if ((_param_fw_arsp_mode.get() == 0) && 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);

@@ -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,
-		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
+		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,

 		(ParamInt<px4::params::FW_ARSP_SCALE_EN>) _param_fw_arsp_scale_en,

@@ -41,18 +41,16 @@
 */

 /**
- * Use airspeed for control
+ * Airspeed mode
 *
- * 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
+ * On vehicles without airspeed sensor this parameter can be used to
+ * enable flying without an airspeed reading
 *
- * @boolean
+ * @value 0 Use airspeed in controller
+ * @value 1 Do not use airspeed in controller
 * @group FW Rate Control
 */
-PARAM_DEFINE_INT32(FW_USE_AIRSPD, 1);
+PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);

 /**
 * Pitch rate proportional gain.
@@ -68,7 +68,6 @@ bool McAutotuneAttitudeControl::init()

 void McAutotuneAttitudeControl::reset()
 {
-	_param_mc_at_start.reset();
 }

 void McAutotuneAttitudeControl::Run()
@@ -322,7 +322,8 @@ 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;
-		_mission_item.yaw = NAN; // FlightTaskAuto handles yaw directly
+		/* hold heading for takeoff items */
+		_mission_item.yaw = _navigator->get_local_position()->heading;
 		_mission_item.altitude = _mission_init_climb_altitude_amsl;
 		_mission_item.altitude_is_relative = false;
 		_mission_item.autocontinue = true;
@@ -365,6 +366,9 @@ 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,
@@ -385,13 +389,16 @@ 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 = NAN;
+		_mission_item.yaw = _navigator->get_local_position()->heading;

 		// keep current setpoints (FW position controller generates wp to track during transition)
 		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
@@ -421,6 +428,9 @@ 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]);
@@ -435,6 +445,9 @@ 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;
@@ -654,6 +654,9 @@ 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) &&
@@ -770,11 +773,13 @@ 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 (!PX4_ISFINITE(pos_sp_triplet->current.yaw)) {
-				_mission_item.yaw = NAN;
+			if (!pos_sp_triplet->current.yaw_valid) {
+				_mission_item.yaw = _navigator->get_local_position()->heading;
 				pos_sp_triplet->current.yaw = _mission_item.yaw;
+				pos_sp_triplet->current.yaw_valid = true;
 			}
 		}

@@ -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
-		    && _navigator->get_yaw_to_be_accepted(_mission_item.yaw)
+		    && PX4_ISFINITE(_navigator->get_yaw_acceptance(_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,6 +423,14 @@ 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) &&
@@ -465,8 +473,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 (with small margin)
-			const bool exit_course_is_reachable = dist_current_next > 1.05f * curr_sp_new->loiter_radius;
+			// 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;

 			if (enforce_exit_course && exit_course_is_reachable) {

@@ -660,6 +668,7 @@ 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;
@@ -695,10 +704,6 @@ 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;
@@ -741,7 +746,6 @@ 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
@@ -761,8 +765,8 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
 	}

 	item->altitude = loiter_altitude_amsl;
+
 	item->loiter_radius = _navigator->get_loiter_radius();
-	item->yaw = NAN;
 }

 void
@@ -770,11 +774,10 @@ MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s
 {
 	setLoiterItemCommonFields(item);

-	_navigator->calculate_breaking_stop(item->lat, item->lon);
+	_navigator->calculate_breaking_stop(item->lat, item->lon, item->yaw);

 	item->altitude = _navigator->get_global_position()->alt;
 	item->loiter_radius = _navigator->get_loiter_radius();
-	item->yaw = NAN;
 }

 void
@@ -798,7 +801,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 = NAN;
+	item->yaw = _navigator->get_local_position()->heading;

 	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
@@ -828,7 +831,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 = NAN;
+	item->yaw = _navigator->get_local_position()->heading;

 	item->altitude = 0;
 	item->altitude_is_relative = false;
@@ -860,7 +863,14 @@ 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; // not immediate transition
+	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->autocontinue = true;
 }

@@ -239,13 +239,14 @@ public:
 	void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }

 	/**
-	 * Get if the yaw acceptance is required at the current mission item
+	 * Get the yaw acceptance given the current mission item
 	 *
 	 * @param mission_item_yaw the yaw to use in case the controller-derived radius is finite
 	 *
-	 * @return true if the yaw acceptance is required, false if not required
+	 * @return the yaw at which the next waypoint should be used or NaN if the yaw at a waypoint
+	 * should be ignored
 	 */
-	bool get_yaw_to_be_accepted(float mission_item_yaw);
+	float get_yaw_acceptance(float mission_item_yaw);

 	orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }

@@ -278,7 +279,7 @@ public:
 	void release_gimbal_control();
 	void set_gimbal_neutral();

-	void calculate_breaking_stop(double &lat, double &lon);
+	void calculate_breaking_stop(double &lat, double &lon, float &yaw);

 	void stop_capturing_images();
 	void disable_camera_trigger();
@@ -309,9 +309,11 @@ 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)) {
@@ -346,7 +348,8 @@ 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);
+							calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
+							rep->current.yaw_valid = true;

 						} else {
 							// For fixedwings we can use the current vehicle's position to define the loiter point
@@ -443,6 +446,7 @@ 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;
@@ -454,7 +458,8 @@ 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);
+						calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
+						rep->current.yaw_valid = true;
 					}

 					if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
@@ -594,18 +599,19 @@ 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;
@@ -1033,7 +1039,8 @@ void Navigator::geofence_breach_check()
 				}

 				rep->current.timestamp = hrt_absolute_time();
-				rep->current.yaw = NAN;
+				rep->current.yaw = get_local_position()->heading;
+				rep->current.yaw_valid = true;
 				rep->current.lat = loiter_latitude;
 				rep->current.lon = loiter_longitude;
 				rep->current.alt = loiter_altitude_amsl;
@@ -1150,13 +1157,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;
 }

@@ -1186,7 +1193,7 @@ float Navigator::get_acceptance_radius()
 	return acceptance_radius;
 }

-bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw)
+float Navigator::get_yaw_acceptance(float mission_item_yaw)
 {
 	float yaw = mission_item_yaw;

@@ -1198,7 +1205,7 @@ bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw)
 		yaw = pos_ctrl_status.yaw_acceptance;
 	}

-	return PX4_ISFINITE(yaw);
+	return yaw;
 }

 void Navigator::load_fence_from_file(const char *filename)
@@ -1463,20 +1470,21 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
 	return true;
 }

-void Navigator::calculate_breaking_stop(double &lat, double &lon)
+void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
 {
 	// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back
-	const float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
+	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);

-	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);
+	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)
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
PerFrivik	a471755dac	Rover module: Temp	2023-12-19 15:04:34 +01:00
Matthias Grob	7cbc6241d7	Add differential drive control module to all targets that have rover support	2023-12-19 11:55:52 +01:00
Matthias Grob	257a66d99d	Differential Rover: PR fixes	2023-12-19 11:54:41 +01:00
Matthias Grob	a26be63f54	differential_drive_control: don't build by default also add dependency on control allocation parameter CA_R_REV	2023-12-19 11:39:36 +01:00
PerFrivik	679a4e2d98	Differential Rover: Differential drive module & library	2023-12-19 11:37:08 +01:00
PerFrivik	ddd201118d	Differential Drive: Added mixing interface for motors	2023-12-19 11:29:27 +01:00
PerFrivik	6e7ed49ad3	Differential Rover: Added logging and dds topics	2023-12-19 11:22:14 +01:00
PerFrivik	a33dccfcc2	Differential Rover: Update airframe architecture	2023-12-19 11:22:05 +01:00