Implemented flush for Serial UART API

Removed commented out code
Fixed processing bug on Qurt platform due to compiler bug
2026-06-10 17:10:06 +08:00 · 2024-03-07 10:27:56 -08:00 · 2024-03-06 16:53:11 -08:00 · 2024-03-06 16:52:20 -08:00 · 2024-03-06 16:30:59 -08:00 · 2024-03-06 08:51:29 -08:00
162 changed files with 5151 additions and 2286 deletions
@@ -120,7 +120,7 @@ pipeline {
                     "px4_fmu-v6xrt_rover",
                     "px4_io-v2_default",
                     "raspberrypi_pico_default",
-                     "siyi_n7_default"
+                     "siyi_n7_default",
                     "sky-drones_smartap-airlink_default",
                     "spracing_h7extreme_default",
                     "thepeach_k1_default",
@@ -0,0 +1,80 @@
+#!/bin/sh
+# @name Gazebo lawnmower
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_differential_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+# We can arm and drive in manual mode when it slides and GPS check fails:
+param set-default COM_ARM_WO_GPS 1
+
+# Set Differential Drive Kinematics Library parameters:
+param set RDD_WHEEL_BASE 0.9
+param set RDD_WHEEL_RADIUS 0.22
+param set RDD_WHEEL_SPEED 10.0	# Maximum wheel speed rad/s, approx 8 km/h
+
+# Actuator mapping - set SITL motors/servos output parameters:
+
+# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+#param set-default SIM_GZ_WH_MIN1 0
+#param set-default SIM_GZ_WH_MAX1 200
+#param set-default SIM_GZ_WH_DIS1 100
+#param set-default SIM_GZ_WH_FAIL1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
+#param set-default SIM_GZ_WH_MIN2 0
+#param set-default SIM_GZ_WH_MAX2 200
+#aram set-default SIM_GZ_WH_DIS2 100
+#param set-default SIM_GZ_WH_FAIL2 100
+
+param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
+
+# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. 
+# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate 
+# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator 
+# controls in practical scenarios.
+
+# Cutter deck blades clutch, PCA9685 servo channel 3,  "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
+param set-default SIM_GZ_SV_FUNC3 203
+param set-default SIM_GZ_SV_MIN3 0
+param set-default SIM_GZ_SV_MAX3 1000
+param set-default SIM_GZ_SV_DIS3 500
+param set-default SIM_GZ_SV_FAIL3 500
+
+# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
+#    - on minimum when disarmed or failed:
+param set-default SIM_GZ_SV_FUNC4 204
+param set-default SIM_GZ_SV_MIN4 0
+param set-default SIM_GZ_SV_MAX4 1000
+param set-default SIM_GZ_SV_DIS4 500
+param set-default SIM_GZ_SV_FAIL4 500
+
+# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:
+
+# Strobes, PCA9685 servo channel 5, "Servo 5" (205) - flashing indicates Mission mode:
+#param set-default SIM_GZ_SV_FUNC5 205
+#param set-default SIM_GZ_SV_MIN5 1000
+#param set-default SIM_GZ_SV_MAX5 2000
+#param set-default SIM_GZ_SV_DIS5 1000
+#param set-default SIM_GZ_SV_FAIL5 1000
+
+# Horn, PCA9685 servo channel 6, "Servo 6" (206) - for alarms like GPS failure:
+#param set-default SIM_GZ_SV_FUNC6 206
+
+# Spare PCA9685 servo channel 7 on "RC AUX2" (408) - right knob, or "Servo 7" (207):
+#param set-default SIM_GZ_SV_FUNC7 207
+
+# Spare PCA9685 servo channel 8 - "Servo 8" (208):
+#param set-default SIM_GZ_SV_FUNC8 208
@@ -82,6 +82,7 @@ px4_add_romfs_files(
 	4008_gz_advanced_plane
 	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
+	4011_gz_lawnmower

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -41,19 +41,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then

 	echo "INFO  [init] Gazebo simulator"

-	# set local coordinate frame reference
-	if [ -n "${PX4_HOME_LAT}" ]; then
-		param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
-	fi
-
-	if [ -n "${PX4_HOME_LON}" ]; then
-		param set SIM_GZ_HOME_LON ${PX4_HOME_LON}
-	fi
-
-	if [ -n "${PX4_HOME_ALT}" ]; then
-		param set SIM_GZ_HOME_ALT ${PX4_HOME_ALT}
-	fi
-
 	# Only start up Gazebo if PX4_GZ_STANDALONE is not set.
 	if [ -z "${PX4_GZ_STANDALONE}" ]; then

@@ -180,6 +167,12 @@ elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)"

 else
 	# otherwise start simulator (mavlink) module
+
+	# EKF2 specifics
+	param set-default EKF2_GPS_DELAY 10
+	param set-default EKF2_MULTI_IMU 3
+	param set-default SENS_IMU_MODE 0
+
 	simulator_tcp_port=$((4560+px4_instance))

 	# Check if PX4_SIM_HOSTNAME environment variable is empty
@@ -164,10 +164,6 @@ param set-default COM_RC_IN_MODE 1
 # Speedup SITL startup
 param set-default EKF2_REQ_GPS_H 0.5

-# Multi-EKF
-param set-default EKF2_MULTI_IMU 3
-param set-default SENS_IMU_MODE 0
-
 param set-default IMU_GYRO_FFT_EN 1
 param set-default MAV_PROTO_VER 2 # Ensures QGC does not drop the first few packets after a SITL restart due to MAVLINK 1 packets

@@ -34,28 +34,79 @@
 add_subdirectory(airframes)

 px4_add_romfs_files(
-	rc.airship_apps
-	rc.airship_defaults
+
 	rc.autostart_ext
-	rc.balloon_apps
-	rc.balloon_defaults
-	rc.boat_defaults
-	rc.fw_apps
-	rc.fw_defaults
-	rc.heli_defaults
-	rc.logging
-	rc.mc_apps
-	rc.mc_defaults
+
 	rcS
 	rc.sensors
-	rc.thermal_cal
-	rc.rover_apps
-	rc.rover_defaults
-	rc.rover_differential_apps
-	rc.rover_differential_defaults
-	rc.uuv_apps
-	rc.uuv_defaults
 	rc.vehicle_setup
-	rc.vtol_apps
-	rc.vtol_defaults
+
+	# TODO
+	rc.balloon_apps
+	rc.balloon_defaults
 )
+
+if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
+	px4_add_romfs_files(
+		rc.airship_apps
+		rc.airship_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_FW_RATE_CONTROL)
+	px4_add_romfs_files(
+		rc.fw_apps
+		rc.fw_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_MC_RATE_CONTROL)
+	px4_add_romfs_files(
+		rc.heli_defaults
+		rc.mc_apps
+		rc.mc_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_ROVER_POS_CONTROL)
+	px4_add_romfs_files(
+		rc.rover_apps
+		rc.rover_defaults
+
+		rc.boat_defaults # hack
+	)
+endif()
+
+if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
+	px4_add_romfs_files(
+		rc.rover_differential_apps
+		rc.rover_differential_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_UUV_ATT_CONTROL)
+	px4_add_romfs_files(
+		rc.uuv_apps
+		rc.uuv_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_VTOL_ATT_CONTROL)
+	px4_add_romfs_files(
+		rc.vtol_apps
+		rc.vtol_defaults
+	)
+endif()
+
+
+if(CONFIG_MODULES_LOGGER)
+	px4_add_romfs_files(
+		rc.logging
+	)
+endif()
+
+if(CONFIG_MODULES_TEMPERATURE_COMPENSATION)
+	px4_add_romfs_files(
+		rc.thermal_cal
+	)
+endif()
@@ -32,94 +32,127 @@
 ############################################################################

 px4_add_romfs_files(
-	# [0-999] Reserved (historical)"
-
-	# [1000, 1999] Simulation setups"
-	1001_rc_quad_x.hil
-	1002_standard_vtol.hil
-	1100_rc_quad_x_sih.hil
-	1101_rc_plane_sih.hil
-	1102_tailsitter_duo_sih.hil
-
-	# [2000, 2999] Standard planes"
-	2100_standard_plane
-	2106_albatross
-
-	2507_cloudship
-
-	# [3000, 3999] Flying wing"
-	3000_generic_wing
-
-	# [4000, 4999] Quadrotor x"
-	4001_quad_x
-	4014_s500
-	4015_holybro_s500
-	4016_holybro_px4vision
-	4017_nxp_hovergames
-	4019_x500_v2
-	4020_holybro_px4vision_v1_5
-	4041_beta75x
-	4050_generic_250
-	4052_holybro_qav250
-	4053_holybro_kopis2
-	4061_atl_mantis_edu
-	4071_ifo
-	4073_ifo-s
-	4500_clover4
-	4901_crazyflie21
-
-	# [5000, 5999] Quadrotor +"
-	5001_quad_+
-
-	# [6000, 6999] Hexarotor x"
-	6001_hexa_x
-	6002_draco_r
-
-	# [7000, 7999] Hexarotor +"
-	7001_hexa_+
-
-	# [8000, 8999] Octorotor +"
-	8001_octo_x
-
-	# [9000, 9999] Octorotor +"
-	9001_octo_+
-
-	# [11000, 11999] Hexa Cox
-	11001_hexa_cox
-
-	# [12000, 12999] Octo Cox
-	12001_octo_cox
-
-	# [13000, 13999] VTOL
-	13000_generic_vtol_standard
-	13100_generic_vtol_tiltrotor
-	13013_deltaquad
-	13014_vtol_babyshark
-	13030_generic_vtol_quad_tiltrotor
-	13200_generic_vtol_tailsitter
-
-	# [14000, 14999] MC with tilt
-	14001_generic_mc_with_tilt
-
-	16001_helicopter
-
-	# [17000, 17999] Autogyro
-	17002_TF-AutoG2
-	17003_TF-G2
+	# [0-999] Reserved (historical)

 	# [18000, 18999] High-altitude balloons
 	18001_TF-B1

 	# [22000, 22999] Reserve for custom models
-
-	24001_dodeca_cox
-
-	50000_generic_ground_vehicle
-	50004_nxpcup_car_dfrobot_gpx
-	50003_aion_robotics_r1_rover
-
-	# [60000, 61000] (Unmanned) Underwater Robots
-	60000_uuv_generic
-	60001_uuv_hippocampus
-	60002_uuv_bluerov2_heavy
 )
+
+if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
+	px4_add_romfs_files(
+		# [1000, 1999] Simulation setups
+		1001_rc_quad_x.hil
+		1002_standard_vtol.hil
+		1100_rc_quad_x_sih.hil
+		1101_rc_plane_sih.hil
+		1102_tailsitter_duo_sih.hil
+	)
+endif()
+
+if(CONFIG_MODULES_MC_RATE_CONTROL)
+	px4_add_romfs_files(
+		# [4000, 4999] Quadrotor x
+		4001_quad_x
+		4014_s500
+		4015_holybro_s500
+		4016_holybro_px4vision
+		4017_nxp_hovergames
+		4019_x500_v2
+		4020_holybro_px4vision_v1_5
+		4041_beta75x
+		4050_generic_250
+		4052_holybro_qav250
+		4053_holybro_kopis2
+		4061_atl_mantis_edu
+		4071_ifo
+		4073_ifo-s
+		4500_clover4
+		4901_crazyflie21
+
+		# [5000, 5999] Quadrotor +
+		5001_quad_+
+
+		# [6000, 6999] Hexarotor x
+		6001_hexa_x
+		6002_draco_r
+
+		# [7000, 7999] Hexarotor +
+		7001_hexa_+
+
+		# [8000, 8999] Octorotor +
+		8001_octo_x
+
+		# [9000, 9999] Octorotor +
+		9001_octo_+
+
+		# [11000, 11999] Hexa Cox
+		11001_hexa_cox
+
+		# [12000, 12999] Octo Cox
+		12001_octo_cox
+
+		# [14000, 14999] MC with tilt
+		14001_generic_mc_with_tilt
+
+		16001_helicopter
+
+		24001_dodeca_cox
+	)
+endif()
+
+if(CONFIG_MODULES_FW_RATE_CONTROL)
+	px4_add_romfs_files(
+		# [2000, 2999] Standard planes
+		2100_standard_plane
+		2106_albatross
+
+		# [3000, 3999] Flying wing
+		3000_generic_wing
+
+		# [17000, 17999] Autogyro
+		17002_TF-AutoG2
+		17003_TF-G2
+	)
+endif()
+
+if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
+	px4_add_romfs_files(
+		2507_cloudship
+	)
+endif()
+
+if(CONFIG_MODULES_VTOL_ATT_CONTROL)
+	px4_add_romfs_files(
+		# [13000, 13999] VTOL
+		13000_generic_vtol_standard
+		13100_generic_vtol_tiltrotor
+		13013_deltaquad
+		13014_vtol_babyshark
+		13030_generic_vtol_quad_tiltrotor
+		13200_generic_vtol_tailsitter
+	)
+endif()
+
+if(CONFIG_MODULES_ROVER_POS_CONTROL)
+	px4_add_romfs_files(
+		50000_generic_ground_vehicle
+		50004_nxpcup_car_dfrobot_gpx
+	)
+endif()
+
+if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
+	px4_add_romfs_files(
+		50003_aion_robotics_r1_rover
+	)
+endif()
+
+if(CONFIG_MODULES_UUV_ATT_CONTROL)
+	px4_add_romfs_files(
+		# [60000, 61000] (Unmanned) Underwater Robots
+		60000_uuv_generic
+		60001_uuv_hippocampus
+		60002_uuv_bluerov2_heavy
+	)
+endif()
@@ -28,7 +28,6 @@ param set-default EKF2_ARSP_THR 8
 param set-default EKF2_FUSE_BETA 1
 param set-default EKF2_GPS_CHECK 21
 param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
 param set-default EKF2_REQ_EPH 10
 param set-default EKF2_REQ_EPV 10
 param set-default EKF2_REQ_HDRIFT 0.5
@@ -5,7 +5,7 @@
 ekf2 start &

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

 # Start Land Detector.
 land_detector start rover
@@ -439,7 +439,12 @@ else
 	#
 	# Start a thermal calibration if required.
 	#
-	. ${R}etc/init.d/rc.thermal_cal
+	set RC_THERMAL_CAL ${R}etc/init.d/rc.thermal_cal
+	if [ -f ${RC_THERMAL_CAL} ]
+	then
+		. ${RC_THERMAL_CAL}
+	fi
+	unset RC_THERMAL_CAL

 	#
 	# Start gimbal to control mounts such as gimbals, disabled by default.
@@ -500,7 +505,12 @@ else
 	#
 	# Start the logger.
 	#
-	. ${R}etc/init.d/rc.logging
+	set RC_LOGGING ${R}etc/init.d/rc.logging
+	if [ -f ${RC_LOGGING} ]
+	then
+		. ${RC_LOGGING}
+	fi
+	unset RC_LOGGING

 	#
 	# Set additional parameters and env variables for selected AUTOSTART.
@@ -64,9 +64,6 @@
 // Hacks for MAVLink RC button input
 #define ATL_MANTIS_RC_INPUT_HACKS

-// Hacks for MAVLink RC button input
-#define ATL_MANTIS_RC_INPUT_HACKS
-
 /*
 * ADC channels
 *
@@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
@@ -4,13 +4,14 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
+CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
+CONFIG_DRIVERS_RC_CRSF_RC=y
 CONFIG_DRIVERS_QSHELL_QURT=y
-CONFIG_DRIVERS_VOXL2_IO=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_EKF2=y
@@ -28,4 +29,5 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_ORB_COMMUNICATOR=y
@@ -49,6 +49,6 @@ add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
-# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)
@@ -989,10 +989,10 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)

 	gps.device_id = device_id.devid;

-	gps.lat = hil_gps.lat;
-	gps.lon = hil_gps.lon;
-	gps.alt = hil_gps.alt;
-	gps.alt_ellipsoid = hil_gps.alt;
+	gps.latitude_deg = hil_gps.lat;
+	gps.longitude_deg = hil_gps.lon;
+	gps.altitude_msl_m = hil_gps.alt;
+	gps.altitude_ellipsoid_m = hil_gps.alt;

 	gps.s_variance_m_s = 0.25f;
 	gps.c_variance_rad = 0.5f;
@@ -18,4 +18,4 @@ CONFIG_MODULES_UUV_POS_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=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_DIFFERENTIAL_DRIVE=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -16,7 +16,7 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	# Skynode: use the "custom participant" config for uxrce_dds_client
 	param set-default UXRCE_DDS_PTCFG 2
@@ -3,7 +3,7 @@
 # board specific MAVLink startup script.
 #------------------------------------------------------------------------------

-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	# Start MAVLink on the UART connected to the mission computer
 	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
@@ -49,7 +49,7 @@ then
 	fi
 fi

-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	#SKYNODE base fmu board orientation

@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
@@ -18,7 +18,7 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 0

-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	# Skynode: use the "custom participant" config for uxrce_dds_client
 	param set-default UXRCE_DDS_PTCFG 2
@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------

 # if skynode base board is detected start Mavlink on Telem2
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z

@@ -56,7 +56,7 @@ then
 fi

 #Start Auterion Power Module selector for Skynode boards
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	pm_selector_auterion start
 else
@@ -93,7 +93,7 @@ else
 		icm20649 -s -R 6 start
 	else
 		# Internal SPI BMI088
-		if ver hwbasecmp 009 010
+		if ver hwbasecmp 009 010 011
 		then
 			bmi088 -A -R 6 -s start
 			bmi088 -G -R 6 -s start
@@ -110,7 +110,7 @@ else
 	fi

 	# Internal SPI bus ICM42688p
-	if ver hwbasecmp 009 010
+	if ver hwbasecmp 009 010 011
 	then
 		icm42688p -R 12 -s start
 	else
@@ -127,7 +127,7 @@ else
 		# Internal SPI bus ICM-42670-P (hard-mounted)
 		icm42670p -R 10 -s start
 	else
-		if ver hwbasecmp 009 010
+		if ver hwbasecmp 009 010 011
 		then
 			icm20602 -R 6 -s start
 		else
@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
@@ -6,21 +6,30 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6"
+CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
+CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
-CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
-CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
-CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
-CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
+CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
@@ -32,14 +41,20 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_COMMON_UWB=y
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -57,11 +72,15 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_UXRCE_DDS_CLIENT=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------

 # if skynode base board is detected start Mavlink on Telem2
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z

@@ -257,7 +257,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_LPWORKSTACKSIZE=2032
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -1,4 +1,9 @@
+CONFIG_MODULES_AIRSPEED_SELECTOR=n
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
+CONFIG_MODULES_FW_ATT_CONTROL=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -6,6 +11,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
+CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=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_DIFFERENTIAL_DRIVE=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_VERBOSE_STATUS=y
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
@@ -173,6 +173,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	Rpm.msg
+	RtlStatus.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
 	SensorAccel.msg
@@ -1,4 +1,8 @@
 uint64 timestamp # time since system start (microseconds)

 float32 speed # [m/s] collective roll-off speed in body x-axis
+bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward
 float32 yaw_rate # [rad/s] yaw rate
+bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward
+
+# TOPICS differential_drive_setpoint differential_drive_control_output
@@ -55,15 +55,9 @@ bool fs_bad_airspeed          #  5 - true if fusion of the airspeed has encounte
 bool fs_bad_sideslip          #  6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 bool fs_bad_optflow_x         #  7 - true if fusion of the optical flow X axis has encountered a numerical error
 bool fs_bad_optflow_y         #  8 - true if fusion of the optical flow Y axis has encountered a numerical error
-bool fs_bad_vel_n             #  9 - true if fusion of the North velocity has encountered a numerical error
-bool fs_bad_vel_e             # 10 - true if fusion of the East velocity has encountered a numerical error
-bool fs_bad_vel_d             # 11 - true if fusion of the Down velocity has encountered a numerical error
-bool fs_bad_pos_n             # 12 - true if fusion of the North position has encountered a numerical error
-bool fs_bad_pos_e             # 13 - true if fusion of the East position has encountered a numerical error
-bool fs_bad_pos_d             # 14 - true if fusion of the Down position has encountered a numerical error
-bool fs_bad_acc_bias          # 15 - true if bad delta velocity bias estimates have been detected
-bool fs_bad_acc_vertical      # 16 - true if bad vertical accelerometer data has been detected
-bool fs_bad_acc_clipping      # 17 - true if delta velocity data contains clipping (asymmetric railing)
+bool fs_bad_acc_bias          #  9 - true if bad delta velocity bias estimates have been detected
+bool fs_bad_acc_vertical      # 10 - true if bad vertical accelerometer data has been detected
+bool fs_bad_acc_clipping      # 11 - true if delta velocity data contains clipping (asymmetric railing)


 # innovation test failures
@@ -0,0 +1,15 @@
+uint64 timestamp                      # time since system start (microseconds)
+
+uint32 safe_points_id 		      # unique ID of active set of safe_point_items
+bool is_evaluation_pending 	      # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading).
+
+bool has_vtol_approach 		      # flag if approaches are defined for current RTL_TYPE parameter setting
+
+uint8 rtl_type	      		      # Type of RTL chosen
+uint8 safe_point_index 		      # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode
+
+uint8 RTL_STATUS_TYPE_NONE=0       # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
+uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
+uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 	# RTL type is following the mission from closest point to mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position
@@ -52,6 +52,7 @@ add_library(px4_platform STATIC
 	shutdown.cpp
 	spi.cpp
 	pab_manifest.c
+	Serial.cpp
 	${SRCS}
 )
 target_link_libraries(px4_platform prebuild_targets px4_work_queue)
@@ -0,0 +1,143 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/Serial.hpp>
+
+namespace device
+{
+
+Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+	       FlowControl flowcontrol) :
+	_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
+{
+	// If no baudrate was specified then set it to a reasonable default value
+	if (baudrate == 0) {
+		(void) _impl.setBaudrate(9600);
+	}
+}
+
+Serial::~Serial()
+{
+}
+
+bool Serial::open()
+{
+	return _impl.open();
+}
+
+bool Serial::isOpen() const
+{
+	return _impl.isOpen();
+}
+
+bool Serial::close()
+{
+	return _impl.close();
+}
+
+ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
+{
+	return _impl.read(buffer, buffer_size);
+}
+
+ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
+}
+
+ssize_t Serial::write(const void *buffer, size_t buffer_size)
+{
+	return _impl.write(buffer, buffer_size);
+}
+
+void Serial::flush()
+{
+	return _impl.flush();
+}
+
+uint32_t Serial::getBaudrate() const
+{
+	return _impl.getBaudrate();
+}
+
+bool Serial::setBaudrate(uint32_t baudrate)
+{
+	return _impl.setBaudrate(baudrate);
+}
+
+ByteSize Serial::getBytesize() const
+{
+	return _impl.getBytesize();
+}
+
+bool Serial::setBytesize(ByteSize bytesize)
+{
+	return _impl.setBytesize(bytesize);
+}
+
+Parity Serial::getParity() const
+{
+	return _impl.getParity();
+}
+
+bool Serial::setParity(Parity parity)
+{
+	return _impl.setParity(parity);
+}
+
+StopBits Serial::getStopbits() const
+{
+	return _impl.getStopbits();
+}
+
+bool Serial::setStopbits(StopBits stopbits)
+{
+	return _impl.setStopbits(stopbits);
+}
+
+FlowControl Serial::getFlowcontrol() const
+{
+	return _impl.getFlowcontrol();
+}
+
+bool Serial::setFlowcontrol(FlowControl flowcontrol)
+{
+	return _impl.setFlowcontrol(flowcontrol);
+}
+
+const char *Serial::getPort() const
+{
+	return _impl.getPort();
+}
+
+} // namespace device
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <SerialImpl.hpp>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device __EXPORT
+{
+
+class Serial
+{
+public:
+	Serial(const char *port, uint32_t baudrate = 57600,
+	       ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
+	       StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
+	virtual ~Serial();
+
+	// Open sets up the port and gets it configured based on desired configuration
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	void flush();
+
+	// If port is already open then the following configuration functions
+	// will reconfigure the port. If the port is not yet open then they will
+	// simply store the configuration in preparation for the port to be opened.
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+	const char *getPort() const;
+
+private:
+	// Disable copy constructors
+	Serial(const Serial &);
+	Serial &operator=(const Serial &);
+
+	// platform implementation
+	SerialImpl _impl;
+};
+
+} // namespace device
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+namespace device
+{
+namespace SerialConfig
+{
+
+
+// ByteSize: number of data bits
+enum class ByteSize {
+	FiveBits  = 5,
+	SixBits   = 6,
+	SevenBits = 7,
+	EightBits = 8,
+};
+
+// Parity: enable parity checking
+enum class Parity {
+	None = 0,
+	Odd  = 1,
+	Even = 2,
+};
+
+// StopBits: number of stop bits
+enum class StopBits {
+	One = 1,
+	Two = 2
+};
+
+// FlowControl: enable flow control
+enum class FlowControl {
+	Disabled = 0,
+	Enabled  = 1,
+};
+
+} // namespace SerialConfig
+} // namespace device
@@ -13,11 +13,21 @@ __END_DECLS
 #define px4_clock_gettime system_clock_gettime
 #endif

-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)

 __BEGIN_DECLS
 __EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
+__END_DECLS

+#else
+
+#define px4_clock_settime system_clock_settime
+
+#endif
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+__BEGIN_DECLS
 __EXPORT int px4_usleep(useconds_t usec);
 __EXPORT unsigned int px4_sleep(unsigned int seconds);
 __EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
@@ -27,7 +37,6 @@ __END_DECLS

 #else

-#define px4_clock_settime system_clock_settime
 #define px4_usleep system_usleep
 #define px4_sleep system_sleep
 #define px4_pthread_cond_timedwait system_pthread_cond_timedwait
@@ -48,6 +48,8 @@

 #include <board_config.h>

+#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
+
 #include <inttypes.h>
 #include <stdbool.h>
 #include <syslog.h>
@@ -57,7 +59,6 @@
 /****************************************************************************
 * Pre-Processor Definitions
 ****************************************************************************/
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)

 typedef struct {
 	hw_base_id_t             hw_base_id; /* The ID of the Base */
@@ -292,12 +293,6 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
 		.mandatory   = 1,
 		.connection  = px4_hw_con_onboard,
 	},
-	{
-		.id          =  PX4_MFT_PM2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
 	{
 		.id          = PX4_MFT_ETHERNET,
 		.present     = 1,
@@ -315,6 +310,52 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
 // BASE ID 10  Skynode QS no USB Alaised to ID 9
 // BASE ID 16  Auterion Skynode RC10, RC11, RC12, RC13 Alaised to ID 0

+// BASE ID 17  Auterion Skynode RC13 with many parts removed
+static const px4_hw_mft_item_t base_configuration_17[] = {
+	{
+		.id          = PX4_MFT_PX4IO,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_USB,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		.id          = PX4_MFT_CAN2,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_CAN3,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		.id          = PX4_MFT_PM2,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_ETHERNET,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_connector,
+	},
+	{
+		.id          = PX4_MFT_T100_ETH,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+

 static px4_hw_mft_list_entry_t mft_lists[] = {
 //  ver_rev
@@ -328,6 +369,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{HW_BASE_ID(9),  base_configuration_9, arraySize(base_configuration_9)},  // Auterion Skynode ver 9
 	{HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)},  // Auterion Skynode ver 10
 	{HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)},  // Auterion Skynode ver 16
+	{HW_BASE_ID(17), base_configuration_17, arraySize(base_configuration_17)},  // Auterion Skynode ver 17
 };

 /************************************************************************************
@@ -0,0 +1,391 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h>
+#include <px4_log.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+#define MODULE_NAME "SerialImpl"
+
+namespace device
+{
+
+SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		       FlowControl flowcontrol) :
+	_baudrate(baudrate),
+	_bytesize(bytesize),
+	_parity(parity),
+	_stopbits(stopbits),
+	_flowcontrol(flowcontrol)
+{
+	if (port) {
+		strncpy(_port, port, sizeof(_port) - 1);
+		_port[sizeof(_port) - 1] = '\0';
+
+	} else {
+		_port[0] = 0;
+	}
+}
+
+SerialImpl::~SerialImpl()
+{
+	if (isOpen()) {
+		close();
+	}
+}
+
+bool SerialImpl::validateBaudrate(uint32_t baudrate)
+{
+	return ((baudrate == 9600) ||
+		(baudrate == 19200) ||
+		(baudrate == 38400) ||
+		(baudrate == 57600) ||
+		(baudrate == 115200) ||
+		(baudrate == 230400) ||
+		(baudrate == 460800) ||
+		(baudrate == 921600));
+}
+
+bool SerialImpl::configure()
+{
+	/* process baud rate */
+	int speed;
+
+	if (! validateBaudrate(_baudrate)) {
+		PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
+		return false;
+	}
+
+	switch (_baudrate) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
+#endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
+		return false;
+	}
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
+		return false;
+	}
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
+		return false;
+	}
+
+	return true;
+}
+
+bool SerialImpl::open()
+{
+	if (isOpen()) {
+		return true;
+	}
+
+	// Open the serial port
+	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open %s err: %d", _port, errno);
+		return false;
+	}
+
+	_serial_fd = serial_fd;
+
+	// Configure the serial port
+	if (! configure()) {
+		PX4_ERR("failed to configure %s err: %d", _port, errno);
+		return false;
+	}
+
+	_open = true;
+
+	return _open;
+}
+
+bool SerialImpl::isOpen() const
+{
+	return _open;
+}
+
+bool SerialImpl::close()
+{
+
+	if (_serial_fd >= 0) {
+		::close(_serial_fd);
+	}
+
+	_serial_fd = -1;
+	_open = false;
+
+	return true;
+}
+
+ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot read from serial device until it has been opened");
+		return -1;
+	}
+
+	int ret = ::read(_serial_fd, buffer, buffer_size);
+
+	if (ret < 0) {
+		PX4_DEBUG("%s read error %d", _port, ret);
+	}
+
+	return ret;
+}
+
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	if (!_open) {
+		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
+		return -1;
+	}
+
+	if (buffer_size < character_count) {
+		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
+		return -1;
+	}
+
+	const hrt_abstime start_time_us = hrt_absolute_time();
+	int total_bytes_read = 0;
+
+	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
+		// Poll for incoming UART data.
+		pollfd fds[1];
+		fds[0].fd = _serial_fd;
+		fds[0].events = POLLIN;
+
+		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
+
+		if (remaining_time <= 0) { break; }
+
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
+
+		if (ret > 0) {
+			if (fds[0].revents & POLLIN) {
+				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;
+}
+
+void SerialImpl::flush()
+{
+	if (_open) {
+		tcflush(_serial_fd, TCIOFLUSH);
+	}
+}
+
+const char *SerialImpl::getPort() const
+{
+	return _port;
+}
+
+uint32_t SerialImpl::getBaudrate() const
+{
+	return _baudrate;
+}
+
+bool SerialImpl::setBaudrate(uint32_t baudrate)
+{
+	if (! validateBaudrate(baudrate)) {
+		PX4_ERR("ERR: invalid baudrate: %lu", baudrate);
+		return false;
+	}
+
+	// check if already configured
+	if ((baudrate == _baudrate) && _open) {
+		return true;
+	}
+
+	_baudrate = baudrate;
+
+	// process baud rate change now if port is already open
+	if (_open) {
+		return configure();
+	}
+
+	return true;
+}
+
+ByteSize SerialImpl::getBytesize() const
+{
+	return _bytesize;
+}
+
+bool SerialImpl::setBytesize(ByteSize bytesize)
+{
+	return bytesize == ByteSize::EightBits;
+}
+
+Parity SerialImpl::getParity() const
+{
+	return _parity;
+}
+
+bool SerialImpl::setParity(Parity parity)
+{
+	return parity == Parity::None;
+}
+
+StopBits SerialImpl::getStopbits() const
+{
+	return _stopbits;
+}
+
+bool SerialImpl::setStopbits(StopBits stopbits)
+{
+	return stopbits == StopBits::One;
+}
+
+FlowControl SerialImpl::getFlowcontrol() const
+{
+	return _flowcontrol;
+}
+
+bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
+{
+	return flowcontrol == FlowControl::Disabled;
+}
+
+} // namespace device
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	void flush();
+
+	const char *getPort() const;
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+private:
+
+	int _serial_fd{-1};
+
+	bool _open{false};
+
+	char _port[32] {};
+
+	uint32_t _baudrate{0};
+
+	ByteSize _bytesize{ByteSize::EightBits};
+	Parity _parity{Parity::None};
+	StopBits _stopbits{StopBits::One};
+	FlowControl _flowcontrol{FlowControl::Disabled};
+
+	bool validateBaudrate(uint32_t baudrate);
+	bool configure();
+
+};
+
+} // namespace device
@@ -3,6 +3,8 @@
 add_library(px4_layer
 		${KERNEL_SRCS}
 		cdc_acm_check.cpp
+		${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+		SerialImpl.cpp
 	)

 target_link_libraries(px4_layer
@@ -15,6 +15,8 @@ add_library(px4_layer
 	usr_board_ctrl.c
 	usr_hrt.cpp
 	usr_mcu_version.cpp
+	${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+	SerialImpl.cpp
 )

 target_link_libraries(px4_layer
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	void flush();
+
+	const char *getPort() const;
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+private:
+
+	int _serial_fd{-1};
+
+	bool _open{false};
+
+	char _port[32] {};
+
+	uint32_t _baudrate{0};
+
+	ByteSize _bytesize{ByteSize::EightBits};
+	Parity _parity{Parity::None};
+	StopBits _stopbits{StopBits::One};
+	FlowControl _flowcontrol{FlowControl::Disabled};
+
+	bool validateBaudrate(uint32_t baudrate);
+	bool configure();
+};
+
+} // namespace device
@@ -46,6 +46,8 @@ add_library(px4_layer
 	drv_hrt.cpp
 	cpuload.cpp
 	print_load.cpp
+	${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+	SerialImpl.cpp
 )
 target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
 target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable
@@ -0,0 +1,391 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h>
+#include <px4_log.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+namespace device
+{
+
+SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		       FlowControl flowcontrol) :
+	_baudrate(baudrate),
+	_bytesize(bytesize),
+	_parity(parity),
+	_stopbits(stopbits),
+	_flowcontrol(flowcontrol)
+{
+	if (port) {
+		strncpy(_port, port, sizeof(_port) - 1);
+		_port[sizeof(_port) - 1] = '\0';
+
+	} else {
+		_port[0] = 0;
+	}
+}
+
+SerialImpl::~SerialImpl()
+{
+	if (isOpen()) {
+		close();
+	}
+}
+
+bool SerialImpl::validateBaudrate(uint32_t baudrate)
+{
+	return ((baudrate == 9600) ||
+		(baudrate == 19200) ||
+		(baudrate == 38400) ||
+		(baudrate == 57600) ||
+		(baudrate == 115200) ||
+		(baudrate == 230400) ||
+		(baudrate == 460800) ||
+		(baudrate == 921600));
+}
+
+bool SerialImpl::configure()
+{
+	/* process baud rate */
+	int speed;
+
+	if (! validateBaudrate(_baudrate)) {
+		PX4_ERR("ERR: unknown baudrate: %u", _baudrate);
+		return false;
+	}
+
+	switch (_baudrate) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
+#endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %d", _baudrate);
+		return false;
+	}
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
+		return false;
+	}
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
+		return false;
+	}
+
+	return true;
+}
+
+bool SerialImpl::open()
+{
+	if (isOpen()) {
+		return true;
+	}
+
+	// Open the serial port
+	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open %s err: %d", _port, errno);
+		return false;
+	}
+
+	_serial_fd = serial_fd;
+
+	// Configure the serial port
+	if (! configure()) {
+		PX4_ERR("failed to configure %s err: %d", _port, errno);
+		return false;
+	}
+
+	_open = true;
+
+	return _open;
+}
+
+bool SerialImpl::isOpen() const
+{
+	return _open;
+}
+
+bool SerialImpl::close()
+{
+
+	if (_serial_fd >= 0) {
+		::close(_serial_fd);
+	}
+
+	_serial_fd = -1;
+	_open = false;
+
+	return true;
+}
+
+ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot read from serial device until it has been opened");
+		return -1;
+	}
+
+	int ret = ::read(_serial_fd, buffer, buffer_size);
+
+	if (ret < 0) {
+		PX4_DEBUG("%s read error %d", _port, ret);
+
+	}
+
+	return ret;
+}
+
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	if (!_open) {
+		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
+		return -1;
+	}
+
+	if (buffer_size < character_count) {
+		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
+		return -1;
+	}
+
+	const hrt_abstime start_time_us = hrt_absolute_time();
+	int total_bytes_read = 0;
+
+	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
+		// Poll for incoming UART data.
+		pollfd fds[1];
+		fds[0].fd = _serial_fd;
+		fds[0].events = POLLIN;
+
+		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
+
+		if (remaining_time <= 0) { break; }
+
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
+
+		if (ret > 0) {
+			if (fds[0].revents & POLLIN) {
+				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;
+}
+
+void SerialImpl::flush()
+{
+	if (_open) {
+		tcflush(_serial_fd, TCIOFLUSH);
+	}
+}
+
+const char *SerialImpl::getPort() const
+{
+	return _port;
+}
+
+uint32_t SerialImpl::getBaudrate() const
+{
+	return _baudrate;
+}
+
+bool SerialImpl::setBaudrate(uint32_t baudrate)
+{
+	if (! validateBaudrate(baudrate)) {
+		PX4_ERR("ERR: invalid baudrate: %u", baudrate);
+		return false;
+	}
+
+	// check if already configured
+	if ((baudrate == _baudrate) && _open) {
+		return true;
+	}
+
+	_baudrate = baudrate;
+
+	// process baud rate change now if port is already open
+	if (_open) {
+		return configure();
+	}
+
+	return true;
+}
+
+ByteSize SerialImpl::getBytesize() const
+{
+	return _bytesize;
+}
+
+bool SerialImpl::setBytesize(ByteSize bytesize)
+{
+	return bytesize == ByteSize::EightBits;
+}
+
+Parity SerialImpl::getParity() const
+{
+	return _parity;
+}
+
+bool SerialImpl::setParity(Parity parity)
+{
+	return parity == Parity::None;
+}
+
+StopBits SerialImpl::getStopbits() const
+{
+	return _stopbits;
+}
+
+bool SerialImpl::setStopbits(StopBits stopbits)
+{
+	return stopbits == StopBits::One;
+}
+
+FlowControl SerialImpl::getFlowcontrol() const
+{
+	return _flowcontrol;
+}
+
+bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
+{
+	return flowcontrol == FlowControl::Disabled;
+}
+
+} // namespace device
@@ -40,15 +40,34 @@
 #include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
 #include <uORB/uORB.h>

+#if defined(CONFIG_MODULES_MUORB_APPS)
+extern "C" { int muorb_init(); }
+#endif
+
 int px4_platform_init(void)
 {
 	hrt_init();

 	px4::WorkQueueManagerStart();

+// MUORB has slightly different startup requirements
+#if defined(CONFIG_MODULES_MUORB_APPS)
+	//Put sleeper in here to allow wq to finish initializing before param_init is called
+	usleep(10000);
+
+	uorb_start();
+
+	muorb_init();
+
+	// Give muorb some time to setup the DSP
+	usleep(100000);
+
+	param_init();
+#else
 	param_init();

 	uorb_start();
+#endif

 	px4_log_initialize();

@@ -50,5 +50,4 @@ add_library(px4 SHARED
 target_link_libraries(px4
 	modules__muorb__slpi
 	${module_libraries}
-	px4_layer
 	)
@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	void flush();
+
+	const char *getPort() const;
+	bool setPort(const char *port);
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+private:
+
+	int _serial_fd{-1};
+
+	bool _open{false};
+
+	char _port[32] {};
+
+	uint32_t _baudrate{0};
+
+	ByteSize _bytesize{ByteSize::EightBits};
+	Parity _parity{Parity::None};
+	StopBits _stopbits{StopBits::One};
+	FlowControl _flowcontrol{FlowControl::Disabled};
+
+	bool validateBaudrate(uint32_t baudrate);
+
+	// Mutex used to lock the read functions
+	//pthread_mutex_t read_mutex;
+
+	// Mutex used to lock the write functions
+	//pthread_mutex_t write_mutex;
+};
+
+} // namespace device
@@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS
 	px4_qurt_impl.cpp
 	main.cpp
 	qurt_log.cpp
+	SerialImpl.cpp
 	)

 add_library(px4_layer
@@ -0,0 +1,335 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <px4_log.h>
+#include <drivers/device/qurt/uart.h>
+#include <drivers/drv_hrt.h>
+
+namespace device
+{
+
+SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		       FlowControl flowcontrol) :
+	_baudrate(baudrate),
+	_bytesize(bytesize),
+	_parity(parity),
+	_stopbits(stopbits),
+	_flowcontrol(flowcontrol)
+{
+	if (port) {
+		strncpy(_port, port, sizeof(_port) - 1);
+		_port[sizeof(_port) - 1] = '\0';
+
+	} else {
+		_port[0] = 0;
+	}
+}
+
+SerialImpl::~SerialImpl()
+{
+	if (isOpen()) {
+		close();
+	}
+}
+
+bool SerialImpl::validateBaudrate(uint32_t baudrate)
+{
+	if ((baudrate != 9600)   &&
+	    (baudrate != 38400)  &&
+	    (baudrate != 57600)  &&
+	    (baudrate != 115200) &&
+	    (baudrate != 230400) &&
+	    (baudrate != 250000) &&
+	    (baudrate != 420000) &&
+	    (baudrate != 460800) &&
+	    (baudrate != 921600) &&
+	    (baudrate != 1000000) &&
+	    (baudrate != 1843200) &&
+	    (baudrate != 2000000)) {
+		return false;
+	}
+
+	return true;
+}
+
+bool SerialImpl::open()
+{
+	// There's no harm in calling open multiple times on the same port.
+	// In fact, that's the only way to change the baudrate
+
+	_open = false;
+	_serial_fd = -1;
+
+	if (! validateBaudrate(_baudrate)) {
+		PX4_ERR("Invalid baudrate: %u", _baudrate);
+		return false;
+	}
+
+	if (_bytesize != ByteSize::EightBits) {
+		PX4_ERR("Qurt platform only supports ByteSize::EightBits");
+		return false;
+	}
+
+	if (_parity != Parity::None) {
+		PX4_ERR("Qurt platform only supports Parity::None");
+		return false;
+	}
+
+	if (_stopbits != StopBits::One) {
+		PX4_ERR("Qurt platform only supports StopBits::One");
+		return false;
+	}
+
+	if (_flowcontrol != FlowControl::Disabled) {
+		PX4_ERR("Qurt platform only supports FlowControl::Disabled");
+		return false;
+	}
+
+	// qurt_uart_open will check validity of port and baudrate
+	int serial_fd = qurt_uart_open(_port, _baudrate);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd);
+		return false;
+
+	} else {
+		PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate);
+	}
+
+	_serial_fd = serial_fd;
+	_open = true;
+
+	return _open;
+}
+
+bool SerialImpl::isOpen() const
+{
+	return _open;
+}
+
+bool SerialImpl::close()
+{
+	// No close defined for qurt uart yet
+	// if (_serial_fd >= 0) {
+	// 	qurt_uart_close(_serial_fd);
+	// }
+
+	_serial_fd = -1;
+	_open = false;
+
+	return true;
+}
+
+ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot read from serial device until it has been opened");
+		return -1;
+	}
+
+	int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500);
+
+	if (ret_read < 0) {
+		PX4_DEBUG("%s read error %d", _port, ret_read);
+
+	}
+
+	return ret_read;
+}
+
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	if (!_open) {
+		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
+		return -1;
+	}
+
+	if (buffer_size < character_count) {
+		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
+		return -1;
+	}
+
+	const hrt_abstime start_time_us = hrt_absolute_time();
+	int total_bytes_read = 0;
+
+	while (total_bytes_read < (int) character_count) {
+
+		if (timeout_us > 0) {
+			const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
+
+			if (elapsed_us >= timeout_us) {
+				// If there was a partial read but not enough to satisfy the minimum then they will be lost
+				// but this really should never happen when everything is working normally.
+				// PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us);
+				// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
+				return total_bytes_read;
+			}
+		}
+
+		int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
+
+		if (current_bytes_read < 0) {
+			// Again, if there was a partial read but not enough to satisfy the minimum then they will be lost
+			// but this really should never happen when everything is working normally.
+			PX4_ERR("%s failed to read uart", __FUNCTION__);
+			// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
+			return -1;
+		}
+
+		// Current bytes read could be zero
+		total_bytes_read += current_bytes_read;
+
+		// If we have at least reached our desired minimum number of characters
+		// then we can return now
+		if (total_bytes_read >= (int) character_count) {
+			return total_bytes_read;
+		}
+
+		// Wait a set amount of time before trying again or the remaining time
+		// until the timeout if we are getting close
+		const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
+		int64_t time_until_timeout = timeout_us - elapsed_us;
+		uint64_t time_to_sleep = 5000;
+
+		if ((time_until_timeout >= 0) &&
+		    (time_until_timeout < (int64_t) time_to_sleep)) {
+			time_to_sleep = time_until_timeout;
+		}
+
+		px4_usleep(time_to_sleep);
+	}
+
+	return -1;
+}
+
+ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot write to serial device until it has been opened");
+		return -1;
+	}
+
+	int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size);
+
+	if (ret_write < 0) {
+		PX4_ERR("%s write error %d", _port, ret_write);
+
+	}
+
+	return ret_write;
+}
+
+void SerialImpl::flush()
+{
+	if (_open) {
+		uint8_t buffer[4];
+		// A read clears out all current data
+		read(buffer, 4);
+	}
+}
+
+const char *SerialImpl::getPort() const
+{
+	return _port;
+}
+
+uint32_t SerialImpl::getBaudrate() const
+{
+	return _baudrate;
+}
+
+bool SerialImpl::setBaudrate(uint32_t baudrate)
+{
+	if (! validateBaudrate(baudrate)) {
+		PX4_ERR("Invalid baudrate: %u", baudrate);
+		return false;
+	}
+
+	// check if already configured
+	if (baudrate == _baudrate) {
+		return true;
+	}
+
+	_baudrate = baudrate;
+
+	// process baud rate change now if port is already open
+	if (_open) {
+		return open();
+	}
+
+	return true;
+}
+
+ByteSize SerialImpl::getBytesize() const
+{
+	return _bytesize;
+}
+
+bool SerialImpl::setBytesize(ByteSize bytesize)
+{
+	return bytesize == ByteSize::EightBits;
+}
+
+Parity SerialImpl::getParity() const
+{
+	return _parity;
+}
+
+bool SerialImpl::setParity(Parity parity)
+{
+	return parity == Parity::None;
+}
+
+StopBits SerialImpl::getStopbits() const
+{
+	return _stopbits;
+}
+
+bool SerialImpl::setStopbits(StopBits stopbits)
+{
+	return stopbits == StopBits::One;
+}
+
+FlowControl SerialImpl::getFlowcontrol() const
+{
+	return _flowcontrol;
+}
+
+bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
+{
+	return flowcontrol == FlowControl::Disabled;
+}
+
+} // namespace device
@@ -81,7 +81,7 @@ static void hrt_unlock()
 	px4_sem_post(&_hrt_lock);
 }

-int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
 {
 	return 0;
 }
@@ -77,6 +77,7 @@ BMP388::init()

 	if (_chip_id == BMP390_CHIP_ID) {
 		_interface->set_device_type(DRV_BARO_DEVTYPE_BMP390);
+		this->_item_name = "bmp390";
 	}

 	_chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR);
@@ -392,6 +392,12 @@ int BATT_SMBUS::get_startup_info()
 	uint16_t state_of_health;
 	ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health);

+	/* ManufacturerAccess dummy command to init the ManufacturerBlockAccess routine
+	in the BQ40Zx0 and avoid timeout during LifetimeDataFlush.
+	test Sleep > 20 ms to give time to init the ManufacturerBlockAccess routine*/
+	ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, BATT_SMBUS_DEVICE_TYPE);
+	px4_usleep(30_ms);
+
 	if (!ret) {
 		_serial_number = serial_num;
 		_batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult);
@@ -107,6 +107,7 @@ using namespace time_literals;

 #define BATT_SMBUS_SECURITY_KEYS                        0x0035

+#define BATT_SMBUS_DEVICE_TYPE                          0x0001
 #define BATT_SMBUS_LIFETIME_FLUSH                       0x002E
 #define BATT_SMBUS_LIFETIME_BLOCK_ONE                   0x0060
 #define BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS        0x4938
@@ -45,7 +45,6 @@
 #include <poll.h>
 #endif

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

 #include <drivers/drv_sensor.h>
@@ -57,6 +56,8 @@
 #include <px4_platform_common/cli.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/Serial.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
@@ -81,6 +82,7 @@
 #include <linux/spi/spidev.h>
 #endif /* __PX4_LINUX */

+using namespace device;
 using namespace time_literals;

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

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

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

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

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

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

 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -449,72 +470,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 #ifdef __PX4_LINUX

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

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

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

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

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

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

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

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

 		case 'j':
-			if (!strcmp(myoptarg, "spi")) {
-				interface_secondary = GPSHelper::Interface::SPI;
-
-			} else if (!strcmp(myoptarg, "uart")) {
+			if (!strcmp(myoptarg, "uart")) {
 				interface_secondary = GPSHelper::Interface::UART;
-
+#ifdef __PX4_LINUX
+			} else if (!strcmp(myoptarg, "spi")) {
+				interface_secondary = GPSHelper::Interface::SPI;
+#endif
 			} else {
 				PX4_ERR("unknown interface for secondary: %s", myoptarg);
 				error_flag = true;
@@ -46,6 +46,4 @@ px4_add_module(

 	MODULE_CONFIG
 		module.yaml
-	DEPENDS
-		rc
 	)
@@ -43,6 +43,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include <string.h>
+#include <px4_log.h>
 #include "QueueBuffer.hpp"
 #include "CrsfParser.hpp"
 #include "Crc8.hpp"
@@ -161,6 +162,8 @@ static float MapF(const float x, const float in_min, const float in_max, const f
 	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }

+#define CONSTRAIN_CHAN(x) ConstrainF(x, CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX)
+
 static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet)
 {
 	uint32_t raw_channels[CRSF_CHANNEL_COUNT];
@@ -169,25 +172,24 @@ static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPac
 	new_packet->message_type = CRSF_MESSAGE_TYPE_RC_CHANNELS;

 	// Decode channel data
-	raw_channels[0] = (data[0] | data[1] << 8) & 0x07FF;
-	raw_channels[1] = (data[1]  >> 3 | data[2] << 5) & 0x07FF;
-	raw_channels[2] = (data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF;
-	raw_channels[3] = (data[4] >> 1 | data[5] << 7) & 0x07FF;
-	raw_channels[4] = (data[5] >> 4 | data[6] << 4) & 0x07FF;
-	raw_channels[5] = (data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF;
-	raw_channels[6] = (data[8] >> 2 | data[9] << 6) & 0x07FF;
-	raw_channels[7] = (data[9] >> 5 | data[10] << 3) & 0x07FF;
-	raw_channels[8] = (data[11] | data[12] << 8) & 0x07FF;
-	raw_channels[9] = (data[12] >> 3 | data[13] << 5) & 0x07FF;
-	raw_channels[10] = (data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF;
-	raw_channels[11] = (data[15] >> 1 | data[16] << 7) & 0x07FF;
-	raw_channels[12] = (data[16] >> 4 | data[17] << 4) & 0x07FF;
-	raw_channels[13] = (data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF;
-	raw_channels[14] = (data[19] >> 2 | data[20] << 6) & 0x07FF;
-	raw_channels[15] = (data[20] >> 5 | data[21] << 3) & 0x07FF;
+	raw_channels[0] = CONSTRAIN_CHAN((data[0] | data[1] << 8) & 0x07FF);
+	raw_channels[1] = CONSTRAIN_CHAN((data[1]  >> 3 | data[2] << 5) & 0x07FF);
+	raw_channels[2] = CONSTRAIN_CHAN((data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF);
+	raw_channels[3] = CONSTRAIN_CHAN((data[4] >> 1 | data[5] << 7) & 0x07FF);
+	raw_channels[4] = CONSTRAIN_CHAN((data[5] >> 4 | data[6] << 4) & 0x07FF);
+	raw_channels[5] = CONSTRAIN_CHAN((data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF);
+	raw_channels[6] = CONSTRAIN_CHAN((data[8] >> 2 | data[9] << 6) & 0x07FF);
+	raw_channels[7] = CONSTRAIN_CHAN((data[9] >> 5 | data[10] << 3) & 0x07FF);
+	raw_channels[8] = CONSTRAIN_CHAN((data[11] | data[12] << 8) & 0x07FF);
+	raw_channels[9] = CONSTRAIN_CHAN((data[12] >> 3 | data[13] << 5) & 0x07FF);
+	raw_channels[10] = CONSTRAIN_CHAN((data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF);
+	raw_channels[11] = CONSTRAIN_CHAN((data[15] >> 1 | data[16] << 7) & 0x07FF);
+	raw_channels[12] = CONSTRAIN_CHAN((data[16] >> 4 | data[17] << 4) & 0x07FF);
+	raw_channels[13] = CONSTRAIN_CHAN((data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF);
+	raw_channels[14] = CONSTRAIN_CHAN((data[19] >> 2 | data[20] << 6) & 0x07FF);
+	raw_channels[15] = CONSTRAIN_CHAN((data[20] >> 5 | data[21] << 3) & 0x07FF);

 	for (i = 0; i < CRSF_CHANNEL_COUNT; i++) {
-		raw_channels[i] = ConstrainF(raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX);
 		new_packet->channel_data.channels[i] = MapF((float)raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX,
 						       1000.0f, 2000.0f);
 	}
@@ -35,8 +35,6 @@
 #include "CrsfParser.hpp"
 #include "Crc8.hpp"

-#include <poll.h>
-#include <termios.h>
 #include <fcntl.h>

 #include <uORB/topics/battery_status.h>
@@ -118,76 +116,78 @@ void CrsfRc::Run()
 {
 	if (should_exit()) {
 		ScheduleClear();
-		::close(_rc_fd);
-		_rc_fd = -1;
+
+		if (_uart) {
+			(void) _uart->close();
+			delete _uart;
+			_uart = nullptr;
+		}
+
 		exit_and_cleanup();
 		return;
 	}

-	if (_rc_fd < 0) {
-		_rc_fd = ::open(_device, O_RDWR | O_NONBLOCK);
+	if (_uart == nullptr) {
+		// Create the UART port instance
+		_uart = new Serial(_device);

-		if (_rc_fd >= 0) {
-			struct termios t;
-
-			tcgetattr(_rc_fd, &t);
-			cfsetspeed(&t, CRSF_BAUDRATE);
-			t.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
-			t.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-			t.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
-			t.c_oflag = 0;
-			tcsetattr(_rc_fd, TCSANOW, &t);
-
-			if (board_rc_swap_rxtx(_device)) {
-#if defined(TIOCSSWAP)
-				ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED);
-#endif // TIOCSSWAP
-			}
-
-			if (board_rc_singlewire(_device)) {
-				_is_singlewire = true;
-#if defined(TIOCSSINGLEWIRE)
-				ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
-#endif // TIOCSSINGLEWIRE
-			}
-
-			PX4_INFO("Crsf serial opened sucessfully");
-
-			if (_is_singlewire) {
-				PX4_INFO("Crsf serial is single wire. Telemetry disabled");
-			}
-
-			tcflush(_rc_fd, TCIOFLUSH);
-
-			Crc8Init(0xd5);
+		if (_uart == nullptr) {
+			PX4_ERR("Error creating serial device %s", _device);
+			px4_sleep(1);
+			return;
 		}
+	}
+
+	if (! _uart->isOpen()) {
+		// Configure the desired baudrate if one was specified by the user.
+		// Otherwise the default baudrate will be used.
+		if (! _uart->setBaudrate(CRSF_BAUDRATE)) {
+			PX4_ERR("Error setting baudrate to %u on %s", CRSF_BAUDRATE, _device);
+			px4_sleep(1);
+			return;
+		}
+
+		// Open the UART. If this is successful then the UART is ready to use.
+		if (! _uart->open()) {
+			PX4_ERR("Error opening serial device  %s", _device);
+			px4_sleep(1);
+			return;
+		}
+
+		if (board_rc_swap_rxtx(_device)) {
+#if defined(TIOCSSWAP)
+			ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+#endif // TIOCSSWAP
+		}
+
+		if (board_rc_singlewire(_device)) {
+			_is_singlewire = true;
+#if defined(TIOCSSINGLEWIRE)
+			ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+#endif // TIOCSSINGLEWIRE
+		}
+
+		PX4_INFO("Crsf serial opened sucessfully");
+
+		if (_is_singlewire) {
+			PX4_INFO("Crsf serial is single wire. Telemetry disabled");
+		}
+
+		_uart->flush();
+
+		Crc8Init(0xd5);

 		_input_rc.rssi_dbm = NAN;
 		_input_rc.link_quality = -1;

 		CrsfParser_Init();
-
-
-	}
-
-	// poll with 100mS timeout
-	pollfd fds[1];
-	fds[0].fd = _rc_fd;
-	fds[0].events = POLLIN;
-	int ret = ::poll(fds, 1, 100);
-
-	if (ret < 0) {
-		PX4_ERR("poll error");
-		// try again with delay
-		ScheduleDelayed(100_ms);
-		return;
 	}

 	const hrt_abstime time_now_us = hrt_absolute_time();
 	perf_count_interval(_cycle_interval_perf, time_now_us);

 	// Read all available data from the serial RC input UART
-	int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+	int new_bytes = _uart->readAtLeast(&_rcs_buf[0], RC_MAX_BUFFER_SIZE, 1, 100);

 	if (new_bytes > 0) {
 		_bytes_rx += new_bytes;
@@ -433,7 +433,8 @@ bool CrsfRc::SendTelemetryBattery(const uint16_t voltage, const uint16_t current
 	write_uint24_t(buf, offset, fuel);
 	write_uint8_t(buf, offset, remaining);
 	WriteFrameCrc(buf, offset, sizeof(buf));
-	return write(_rc_fd, buf, offset) == offset;
+	return _uart->write((void *) buf, (size_t) offset);
+
 }

 bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed,
@@ -449,7 +450,7 @@ bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, c
 	write_uint16_t(buf, offset, altitude);
 	write_uint8_t(buf, offset, num_satellites);
 	WriteFrameCrc(buf, offset, sizeof(buf));
-	return write(_rc_fd, buf, offset) == offset;
+	return _uart->write((void *) buf, (size_t) offset);
 }

 bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw)
@@ -461,7 +462,7 @@ bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, cons
 	write_uint16_t(buf, offset, roll);
 	write_uint16_t(buf, offset, yaw);
 	WriteFrameCrc(buf, offset, sizeof(buf));
-	return write(_rc_fd, buf, offset) == offset;
+	return _uart->write((void *) buf, (size_t) offset);
 }

 bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
@@ -480,7 +481,7 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
 	offset += length;
 	buf[offset - 1] = 0; // ensure null-terminated string
 	WriteFrameCrc(buf, offset, length + 4);
-	return write(_rc_fd, buf, offset) == offset;
+	return _uart->write((void *) buf, (size_t) offset);
 }

 int CrsfRc::print_status()
@@ -40,6 +40,7 @@
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
+#include <px4_platform_common/Serial.hpp>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <drivers/drv_hrt.h>
 #include <lib/perf/perf_counter.h>
@@ -53,6 +54,8 @@
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_status.h>

+using namespace device;
+
 class CrsfRc : public ModuleBase<CrsfRc>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -87,7 +90,8 @@ private:

 	bool SendTelemetryFlightMode(const char *flight_mode);

-	int _rc_fd{-1};
+	Serial *_uart = nullptr; ///< UART interface to RC
+
 	char _device[20] {}; ///< device / serial port path
 	bool _is_singlewire{false};

@@ -45,6 +45,7 @@ px4_add_module(
 		voxl2_io.cpp
 		voxl2_io.hpp
 	DEPENDS
+		rc
 		px4_work_queue
 		mixer_module
 	MODULE_CONFIG
@@ -42,7 +42,7 @@ class GeoTest : public ::testing::Test
 public:
 	void SetUp() override
 	{
-		proj.initReference(math::radians(473566094 / 1e7), math::radians(85190237 / 1e7), 0);
+		proj.initReference(473566094 / 1e7, 85190237 / 1e7, 0);
 	}

 protected:
@@ -73,7 +73,7 @@ TEST_F(GeoTest, reprojectProject)

 TEST_F(GeoTest, projectReproject)
 {
-	// GIVEN: x and y coordinates in the local cartesian frame
+	// GIVEN: lat and lon coordinates in the geographic coordinate system
 	double lat = 47.356616973876953;
 	double lon = 8.5190505981445313;
 	float x;
@@ -391,12 +391,6 @@ public:
 			for (unsigned j = 0; j < N; j++) {
 				double d = static_cast<double>(self(i, j));

-				// Matrix diagonal elements
-				if (N > 1 && M > 1 && i == j) {
-					// make diagonal elements bold (ANSI CSI n 1)
-					printf("\033[1m");
-				}
-
 				// if symmetric don't print upper triangular elements
 				if ((M == N) && (j > i) && (i < N) && (j < M)
 				    && (fabs(d - static_cast<double>(self(j, i))) < (double)eps)
@@ -417,12 +411,6 @@ public:
 						printf("% 6.5f ", d);
 					}
 				}
-
-				// Matrix diagonal elements
-				if (N > 1 && M > 1 && i == j) {
-					// reset any formatting (ANSI CSI n 0)
-					printf("\033[0m");
-				}
 			}

 			printf("\n");
@@ -14,7 +14,7 @@ class XMLInject():
    def __init__(self, injected_xml_filename):
        self.groups=[]

-        valid_parameter_attributes = set(["category", "default", "name", "type", "volatile"])
+        valid_parameter_attributes = set(["category", "default", "name", "type", "volatile", "boolean"])
        valid_field_tags = set(["board","short_desc", "long_desc", "min", "max", "unit", "decimal", "increment", "reboot_required"])
        valid_other_top_level_tags = set(["group","values"])

@@ -42,7 +42,8 @@ class XMLInject():
                        new_param.default = iparam.get('default')
                    elif param_attrib == 'volatile':
                        new_param.SetVolatile()
-
+                    elif param_attrib == "boolean":
+                        new_param.SetBoolean()

                #get param info stored as child tags
                for child in iparam:
@@ -1006,8 +1006,8 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 			return PX4_ERROR;
 		}

-		calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees",
-					 (double)math::radians(heading_radians));
+		calibration_log_info(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees",
+				     (double)math::degrees(heading_radians));

 		matrix::Eulerf euler{matrix::Quatf{attitude.q}};
 		euler(2) = heading_radians;
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,17 +31,22 @@
 #
 ############################################################################

+add_subdirectory(DifferentialDriveControl)
+add_subdirectory(DifferentialDriveGuidance)
 add_subdirectory(DifferentialDriveKinematics)

 px4_add_module(
-	MODULE modules__differential_drive_control
-	MAIN differential_drive_control
+	MODULE modules__differential_drive
+	MAIN differential_drive
 	SRCS
-		DifferentialDriveControl.cpp
-		DifferentialDriveControl.hpp
+		DifferentialDrive.cpp
+		DifferentialDrive.hpp
 	DEPENDS
+		DifferentialDriveControl
+		DifferentialDriveGuidance
 		DifferentialDriveKinematics
 		px4_work_queue
+		modules__control_allocator # for parameter CA_R_REV
 	MODULE_CONFIG
 		module.yaml
 )
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -31,38 +31,37 @@
 *
 ****************************************************************************/

-#include "DifferentialDriveControl.hpp"
+#include "DifferentialDrive.hpp"

-using namespace time_literals;
-using namespace matrix;
-namespace differential_drive_control
-{
-
-DifferentialDriveControl::DifferentialDriveControl() :
+DifferentialDrive::DifferentialDrive() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	updateParams();
 }

-bool DifferentialDriveControl::init()
+bool DifferentialDrive::init()
 {
 	ScheduleOnInterval(10_ms); // 100 Hz
 	return true;
 }

-void DifferentialDriveControl::updateParams()
+void DifferentialDrive::updateParams()
 {
 	ModuleParams::updateParams();
-	_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
-	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);

 	_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
+
+	_max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get();
+	_differential_drive_guidance.setMaxSpeed(_max_speed);
 	_differential_drive_kinematics.setMaxSpeed(_max_speed);
+
+	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
+	_differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity);
 	_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
 }

-void DifferentialDriveControl::Run()
+void DifferentialDrive::Run()
 {
 	if (should_exit()) {
 		ScheduleClear();
@@ -70,20 +69,32 @@ void DifferentialDriveControl::Run()
 	}

 	hrt_abstime now = hrt_absolute_time();
+	const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
+	_time_stamp_last = now;

 	if (_parameter_update_sub.updated()) {
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
 		updateParams();
 	}

 	if (_vehicle_control_mode_sub.updated()) {
-		vehicle_control_mode_s vehicle_control_mode;
+		vehicle_control_mode_s vehicle_control_mode{};

 		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
-			_armed = vehicle_control_mode.flag_armed;
-			_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
+			_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
+			_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
+		}
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s);
+			_differential_drive_kinematics.setArmed(spooled_up);
+			_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
 		}
 	}

@@ -94,43 +105,42 @@ void DifferentialDriveControl::Run()
 			manual_control_setpoint_s manual_control_setpoint{};

 			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
-				_differential_drive_setpoint.timestamp = now;
-				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
-				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
-									_max_angular_velocity;
-				_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
+				differential_drive_setpoint_s setpoint{};
+				setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed;
+				setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity;
+
+				// if acro mode, we activate the yaw rate control
+				if (_acro_driving) {
+					setpoint.closed_loop_speed_control = false;
+					setpoint.closed_loop_yaw_rate_control = true;
+
+				} else {
+					setpoint.closed_loop_speed_control = false;
+					setpoint.closed_loop_yaw_rate_control = false;
+				}
+
+				setpoint.timestamp = now;
+				_differential_drive_setpoint_pub.publish(setpoint);
 			}
 		}
+
+	} else if (_mission_driving) {
+		// Mission mode
+		// directly receive setpoints from the guidance library
+		_differential_drive_guidance.computeGuidance(
+			_differential_drive_control.getVehicleYaw(),
+			_differential_drive_control.getVehicleBodyYawRate(),
+			dt
+		);
 	}

-	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
-
-	// publish data to actuator_motors (output module)
-	// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
-	Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
-					_differential_drive_setpoint.speed,
-					_differential_drive_setpoint.yaw_rate);
-
-	// Check if max_angular_wheel_speed is zero
-	const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
-	const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
-
-	if (!_armed || setpoint_timeout || !valid_max_speed) {
-		wheel_speeds = {}; // stop
-	}
-
-	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
-
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
-	wheel_speeds.copyTo(actuator_motors.control);
-	actuator_motors.timestamp = now;
-	_actuator_motors_pub.publish(actuator_motors);
+	_differential_drive_control.control(dt);
+	_differential_drive_kinematics.allocate();
 }

-int DifferentialDriveControl::task_spawn(int argc, char *argv[])
+int DifferentialDrive::task_spawn(int argc, char *argv[])
 {
-	DifferentialDriveControl *instance = new DifferentialDriveControl();
+	DifferentialDrive *instance = new DifferentialDrive();

 	if (instance) {
 		_object.store(instance);
@@ -151,12 +161,12 @@ int DifferentialDriveControl::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }

-int DifferentialDriveControl::custom_command(int argc, char *argv[])
+int DifferentialDrive::custom_command(int argc, char *argv[])
 {
 	return print_usage("unknown command");
 }

-int DifferentialDriveControl::print_usage(const char *reason)
+int DifferentialDrive::print_usage(const char *reason)
 {
 	if (reason) {
 		PX4_ERR("%s\n", reason);
@@ -168,15 +178,13 @@ int DifferentialDriveControl::print_usage(const char *reason)
 Rover Differential Drive controller.
 )DESCR_STR");

-	PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller");
+	PRINT_MODULE_USAGE_NAME("differential_drive", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	return 0;
 }

-extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[])
+extern "C" __EXPORT int differential_drive_main(int argc, char *argv[])
 {
-	return DifferentialDriveControl::main(argc, argv);
+	return DifferentialDrive::main(argc, argv);
 }
-
-} // namespace differential_drive_control
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -33,41 +33,31 @@

 #pragma once

-// PX4 includes
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
-// uORB includes
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/differential_drive_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>

-// Standard library includes
-#include <math.h>
+#include "DifferentialDriveControl/DifferentialDriveControl.hpp"
+#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp"
+#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp"

-// Local includes
-#include <DifferentialDriveKinematics.hpp>
+using namespace time_literals;

-namespace differential_drive_control
-{
-
-class DifferentialDriveControl : public ModuleBase<DifferentialDriveControl>, public ModuleParams,
+class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModuleParams,
 	public px4::ScheduledWorkItem
 {
 public:
-	DifferentialDriveControl();
-	~DifferentialDriveControl() override = default;
+	DifferentialDrive();
+	~DifferentialDrive() override = default;

 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -85,32 +75,31 @@ protected:

 private:
 	void Run() override;
-
-	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};

-	differential_drive_setpoint_s _differential_drive_setpoint{};
-	DifferentialDriveKinematics _differential_drive_kinematics{};
-
-	bool _armed = false;
 	bool _manual_driving = false;
+	bool _mission_driving = false;
+	bool _acro_driving = false;
+	hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
+
+	DifferentialDriveGuidance _differential_drive_guidance{this};
+	DifferentialDriveControl _differential_drive_control{this};
+	DifferentialDriveKinematics _differential_drive_kinematics{this};
+
 	float _max_speed{0.f};
 	float _max_angular_velocity{0.f};

 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
 		(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
-		(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
+		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
 		(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
+		(ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_wheel_speed,
 		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
 	)
 };
-
-} // namespace differential_drive_control
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(DifferentialDriveControl
+	DifferentialDriveControl.cpp
+)
+
+target_link_libraries(DifferentialDriveControl PUBLIC pid)
+target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "DifferentialDriveControl.hpp"
+
+using namespace matrix;
+
+DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+void DifferentialDriveControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_pid_angular_velocity,
+			   _param_rdd_p_gain_angular_velocity.get(), // Proportional gain
+			   _param_rdd_i_gain_angular_velocity.get(), // Integral gain
+			   0, // Derivative gain
+			   20, // Integral limit
+			   200); // Output limit
+
+	pid_set_parameters(&_pid_speed,
+			   _param_rdd_p_gain_speed.get(), // Proportional gain
+			   _param_rdd_i_gain_speed.get(), // Integral gain
+			   0, // Derivative gain
+			   2, // Integral limit
+			   200); // Output limit
+}
+
+void DifferentialDriveControl::control(float dt)
+{
+	if (_vehicle_angular_velocity_sub.updated()) {
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+
+		if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
+			_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
+		}
+	}
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+
+		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+			_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
+			_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
+		}
+	}
+
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+
+		if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
+			Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+			Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+			_vehicle_forward_speed = velocity_in_body_frame(0);
+		}
+	}
+
+	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
+
+	// PID to reach setpoint using control_output
+	differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint;
+
+	if (_differential_drive_setpoint.closed_loop_speed_control) {
+		differential_drive_control_output.speed +=
+			pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt);
+	}
+
+	if (_differential_drive_setpoint.closed_loop_yaw_rate_control) {
+		differential_drive_control_output.yaw_rate +=
+			pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
+	}
+
+	differential_drive_control_output.timestamp = hrt_absolute_time();
+	_differential_drive_control_output_pub.publish(differential_drive_control_output);
+}
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file DifferentialDriveControl.hpp
+ *
+ * Controller for heading rate and forward speed.
+ */
+
+#pragma once
+
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/differential_drive_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+class DifferentialDriveControl : public ModuleParams
+{
+public:
+	DifferentialDriveControl(ModuleParams *parent);
+	~DifferentialDriveControl() = default;
+
+	void control(float dt);
+	float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
+	float getVehicleYaw() const { return _vehicle_yaw; }
+
+protected:
+	void updateParams() override;
+
+private:
+	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
+
+	differential_drive_setpoint_s _differential_drive_setpoint{};
+
+	matrix::Quatf _vehicle_attitude_quaternion{};
+	float _vehicle_yaw{0.f};
+
+	// States
+	float _vehicle_body_yaw_rate{0.f};
+	float _vehicle_forward_speed{0.f};
+
+	PID_t _pid_angular_velocity; ///< The PID controller for yaw rate.
+	PID_t _pid_speed; ///< The PID controller for velocity.
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
+		(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
+		(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
+		(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity
+	)
+};
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(DifferentialDriveGuidance
+	DifferentialDriveGuidance.cpp
+)
+
+target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 "DifferentialDriveGuidance.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+
+	pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt)
+{
+	if (_position_setpoint_triplet_sub.updated()) {
+		_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
+	}
+
+	if (_vehicle_global_position_sub.updated()) {
+		_vehicle_global_position_sub.copy(&_vehicle_global_position);
+	}
+
+	matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon);
+	matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon);
+	matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon);
+
+	const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1),
+					  current_waypoint(0),
+					  current_waypoint(1));
+
+	float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
+				current_waypoint(1));
+	float heading_error = matrix::wrap_pi(desired_heading - yaw);
+
+	if (_current_waypoint != current_waypoint) {
+		_currentState = GuidanceState::TURNING;
+	}
+
+	// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
+	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
+		_currentState = GuidanceState::GOAL_REACHED;
+	}
+
+	float desired_speed = 0.f;
+
+	switch (_currentState) {
+	case GuidanceState::TURNING:
+		desired_speed = 0.f;
+
+		if (fabsf(heading_error) < 0.05f) {
+			_currentState = GuidanceState::DRIVING;
+		}
+
+		break;
+
+	case GuidanceState::DRIVING: {
+			const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
+						   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
+			_forwards_velocity_smoothing.updateDurations(max_velocity);
+			_forwards_velocity_smoothing.updateTraj(dt);
+			desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
+					_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
+			break;
+		}
+
+	case GuidanceState::GOAL_REACHED:
+		// temporary till I find a better way to stop the vehicle
+		desired_speed = 0.f;
+		heading_error = 0.f;
+		angular_velocity = 0.f;
+		_desired_angular_velocity = 0.f;
+		break;
+	}
+
+	// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
+	float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0,
+				     dt) + heading_error;
+
+	differential_drive_setpoint_s output{};
+	output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
+	output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
+	output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true;
+	output.timestamp = hrt_absolute_time();
+
+	_differential_drive_setpoint_pub.publish(output);
+
+	_current_waypoint = current_waypoint;
+}
+
+void DifferentialDriveGuidance::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_heading_p_controller,
+			   _param_rdd_p_gain_heading.get(),  // Proportional gain
+			   0,  // Integral gain
+			   0,  // Derivative gain
+			   0,  // Integral limit
+			   200);  // Output limit
+
+	_forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get());
+	_forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get());
+	_forwards_velocity_smoothing.setMaxVel(_max_speed);
+}
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 <px4_platform_common/module_params.h>
+
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+
+#include <lib/motion_planning/PositionSmoothing.hpp>
+#include <lib/motion_planning/VelocitySmoothing.hpp>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/differential_drive_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+#include <lib/pid/pid.h>
+
+
+/**
+ * @brief Enum class for the different states of guidance.
+ */
+enum class GuidanceState {
+	TURNING, ///< The vehicle is currently turning.
+	DRIVING, ///< The vehicle is currently driving straight.
+	GOAL_REACHED ///< The vehicle has reached its goal.
+};
+
+/**
+ * @brief Class for differential drive guidance.
+ */
+class DifferentialDriveGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for DifferentialDriveGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	DifferentialDriveGuidance(ModuleParams *parent);
+	~DifferentialDriveGuidance() = default;
+
+	/**
+	 * @brief Compute guidance for the vehicle.
+	 * @param global_pos The global position of the vehicle in degrees.
+	 * @param current_waypoint The current waypoint the vehicle is heading towards in degrees.
+	 * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees.
+	 * @param vehicle_yaw The yaw orientation of the vehicle in radians.
+	 * @param body_velocity The velocity of the vehicle in m/s.
+	 * @param angular_velocity The angular velocity of the vehicle in rad/s.
+	 * @param dt The time step in seconds.
+	 */
+	void computeGuidance(float yaw, float angular_velocity, float dt);
+
+	/**
+	 * @brief Set the maximum speed for the vehicle.
+	 * @param max_speed The maximum speed in m/s.
+	 * @return The set maximum speed in m/s.
+	 */
+	float setMaxSpeed(float max_speed) { return _max_speed = max_speed; }
+
+
+	/**
+	 * @brief Set the maximum angular velocity for the vehicle.
+	 * @param max_angular_velocity The maximum angular velocity in rad/s.
+	 * @return The set maximum angular velocity in rad/s.
+	 */
+	float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; }
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
+
+	position_setpoint_triplet_s _position_setpoint_triplet{};
+	vehicle_global_position_s _vehicle_global_position{};
+
+	GuidanceState _currentState; ///< The current state of guidance.
+
+	float _desired_angular_velocity; ///< The desired angular velocity.
+
+	float _max_speed; ///< The maximum speed.
+	float _max_angular_velocity; ///< The maximum angular velocity.
+
+	matrix::Vector2d _current_waypoint; ///< The current waypoint.
+
+	VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion.
+	PositionSmoothing _position_smoothing; ///< The position smoothing.
+
+	PID_t _heading_p_controller; ///< The PID controller for yaw rate.
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_P_HEADING>) _param_rdd_p_gain_heading,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
+		(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
+	)
+};
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -37,4 +37,4 @@ px4_add_library(DifferentialDriveKinematics

 target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

-px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
+px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2023-2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -36,8 +36,38 @@
 #include <mathlib/mathlib.h>

 using namespace matrix;
+using namespace time_literals;

-matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
+DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent)
+{}
+
+void DifferentialDriveKinematics::allocate()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_differential_drive_control_output_sub.updated()) {
+		_differential_drive_control_output_sub.copy(&_differential_drive_control_output);
+	}
+
+	const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now;
+
+	Vector2f wheel_speeds =
+		computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate);
+
+	if (!_armed || setpoint_timeout) {
+		wheel_speeds = {}; // stop
+	}
+
+	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
+
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
+	wheel_speeds.copyTo(actuator_motors.control);
+	actuator_motors.timestamp = now;
+	_actuator_motors_pub.publish(actuator_motors);
+}
+
+matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
 {
 	if (_max_speed < FLT_EPSILON) {
 		return Vector2f();
@@ -54,7 +84,7 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin

 	if (combined_velocity > _max_speed) {
 		float excess_velocity = fabsf(combined_velocity - _max_speed);
-		float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
+		const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
 		gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
 	}

@@ -65,4 +95,3 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin
 	return Vector2f(linear_velocity_x - rotational_velocity,
 			linear_velocity_x + rotational_velocity) / _max_speed;
 }
-
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -34,6 +34,11 @@
 #pragma once

 #include <matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/differential_drive_setpoint.h>

 /**
 * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
@@ -41,10 +46,10 @@
 * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
 * given linear velocity and yaw rate.
 */
-class DifferentialDriveKinematics
+class DifferentialDriveKinematics : public ModuleParams
 {
 public:
-	DifferentialDriveKinematics() = default;
+	DifferentialDriveKinematics(ModuleParams *parent);
 	~DifferentialDriveKinematics() = default;

 	/**
@@ -68,6 +73,10 @@ public:
 	 */
 	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };

+	void setArmed(const bool armed) { _armed = armed; };
+
+	void allocate();
+
 	/**
 	 * @brief Computes the inverse kinematics for differential drive.
 	 *
@@ -75,10 +84,20 @@ public:
 	 * @param yaw_rate Yaw rate of the robot.
 	 * @return matrix::Vector2f Motor velocities for the right and left motors.
 	 */
-	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
+	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const;

 private:
+	uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)};
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+
+	differential_drive_setpoint_s _differential_drive_control_output{};
+	bool _armed = false;
+
 	float _wheel_base{0.f};
 	float _max_speed{0.f};
 	float _max_angular_velocity{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2023-2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -37,9 +37,14 @@

 using namespace matrix;

-TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
+class DifferentialDriveKinematicsTest : public ::testing::Test
+{
+public:
+	DifferentialDriveKinematics kinematics{nullptr};
+};
+
+TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -49,9 +54,8 @@ TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
 }


-TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
+TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(0.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -61,9 +65,8 @@ TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
 }


-TEST(DifferentialDriveKinematicsTest, UnitCase)
+TEST_F(DifferentialDriveKinematicsTest, UnitCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -73,9 +76,8 @@ TEST(DifferentialDriveKinematicsTest, UnitCase)
 }


-TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
+TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -85,9 +87,8 @@ TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
 }


-TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
+TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -96,9 +97,8 @@ TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
 }

-TEST(DifferentialDriveKinematicsTest, RandomCase)
+TEST_F(DifferentialDriveKinematicsTest, RandomCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(2.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -107,9 +107,8 @@ TEST(DifferentialDriveKinematicsTest, RandomCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
 }

-TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
+TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -118,9 +117,8 @@ TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
 }

-TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
+TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -129,9 +127,8 @@ TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
 }

-TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
+TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(FLT_MIN);
 	kinematics.setMaxSpeed(FLT_MIN);
 	kinematics.setMaxAngularVelocity(FLT_MIN);
@@ -140,9 +137,8 @@ TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
 }

-TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -151,9 +147,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
 }

-TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -162,9 +157,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
 }

-TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(2.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -1,5 +1,5 @@
-menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL
-	bool "differential_drive_control"
+menuconfig MODULES_DIFFERENTIAL_DRIVE
+	bool "differential_drive"
 	default n
 	depends on MODULES_CONTROL_ALLOCATOR
 	---help---
@@ -0,0 +1,122 @@
+module_name: Differential Drive
+
+parameters:
+  - group: Rover Differential Drive
+    definitions:
+      RDD_WHEEL_BASE:
+        description:
+          short: Wheel base
+          long: Distance from the center of the right wheel to the center of the left wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+      RDD_WHEEL_RADIUS:
+        description:
+          short: Wheel radius
+          long: Size of the wheel, half the diameter of the wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.1
+      RDD_SPEED_SCALE:
+        description:
+          short: Manual speed scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_ANG_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_WHEEL_SPEED:
+        description:
+          short: Maximum wheel speed
+        type: float
+        unit: rad/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.3
+      RDD_P_HEADING:
+        description:
+          short: Proportional gain for heading controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_P_SPEED:
+        description:
+          short: Proportional gain for speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_I_SPEED:
+        description:
+          short: Integral gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0
+      RDD_P_ANG_VEL:
+        description:
+          short: Proportional gain for angular velocity controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_I_ANG_VEL:
+        description:
+          short: Integral gain for angular velocity controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0
+      RDD_MAX_JERK:
+        description:
+          short: Maximum jerk
+          long: Limit for forwards acc/deceleration change.
+        type: float
+        unit: m/s^3
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5
+      RDD_MAX_ACCEL:
+        description:
+          short: Maximum acceleration
+          long: Maximum acceleration is used to limit the acceleration of the rover
+        type: float
+        unit: m/s^2
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5
@@ -1,55 +0,0 @@
-module_name: Differential Drive Control
-
-parameters:
-  - group: Rover Differential Drive
-    definitions:
-      RDD_WHEEL_BASE:
-        description:
-          short: Wheel base
-          long: Distance from the center of the right wheel to the center of the left wheel
-        type: float
-        unit: m
-        min: 0.001
-        max: 100
-        increment: 0.001
-        decimal: 3
-        default: 0.5
-      RDD_WHEEL_RADIUS:
-        description:
-          short: Wheel radius
-          long: Size of the wheel, half the diameter of the wheel
-        type: float
-        unit: m
-        min: 0.001
-        max: 100
-        increment: 0.001
-        decimal: 3
-        default: 0.1
-      RDD_SPEED_SCALE:
-        description:
-          short: Manual speed scale
-        type: float
-        min: 0
-        max: 1.0
-        increment: 0.01
-        decimal: 2
-        default: 1.0
-      RDD_ANG_SCALE:
-        description:
-          short: Manual angular velocity scale
-        type: float
-        min: 0
-        max: 1.0
-        increment: 0.01
-        decimal: 2
-        default: 1.0
-      RDD_WHL_SPEED:
-        description:
-          short: Maximum wheel speed
-        type: float
-        unit: rad/s
-        min: 0
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: 10
@@ -127,7 +127,8 @@ list(APPEND EKF_SRCS
 	EKF/height_control.cpp
 	EKF/imu_down_sampler.cpp
 	EKF/output_predictor.cpp
-	EKF/vel_pos_fusion.cpp
+	EKF/velocity_fusion.cpp
+	EKF/position_fusion.cpp
 	EKF/yaw_fusion.cpp
 	EKF/zero_innovation_heading_update.cpp

@@ -44,7 +44,8 @@ list(APPEND EKF_SRCS
 	height_control.cpp
 	imu_down_sampler.cpp
 	output_predictor.cpp
-	vel_pos_fusion.cpp
+	velocity_fusion.cpp
+	position_fusion.cpp
 	yaw_fusion.cpp
 	zero_innovation_heading_update.cpp

@@ -60,22 +60,13 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 		if (zero_gyro_update_data_ready) {

 			Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt;
-			Vector3f innovation = ekf.state().gyro_bias - gyro_bias;

 			const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f));

-			const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var;
-
-			for (int i = 0; i < 3; i++) {
-				Ekf::VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
-				const unsigned state_index = State::gyro_bias.idx + i;
-
-				// calculate kalman gain K = PHS, where S = 1/innovation variance
-				for (int row = 0; row < State::size; row++) {
-					K(row) = ekf.stateCovariance(row, state_index) / innov_var(i);
-				}
-
-				ekf.measurementUpdate(K, innov_var(i), innovation(i));
+			for (unsigned i = 0; i < 3; i++) {
+				const float innovation = ekf.state().gyro_bias(i) - gyro_bias(i);
+				const float innov_var = ekf.getGyroBiasVariance()(i) + obs_var;
+				ekf.fuseDirectStateMeasurement(innovation, innov_var, obs_var, State::gyro_bias.idx + i);
 			}

 			// Reset the integrators
@@ -66,7 +66,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye

 			for (unsigned i = 0; i < 3; i++) {
 				const float innovation = ekf.state().vel(i) - vel_obs(i);
-				ekf.fuseVelPosHeight(innovation, innov_var(i), State::vel.idx + i);
+				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
 			}

 			_time_last_zero_velocity_fuse = imu_delayed.time_us;
@@ -93,7 +93,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
 		const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
 		const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant
-		                                         && (is_airspeed_consistent || !_control_status.flags.wind); // if wind isn't already estimated, the states are reset when starting airspeed fusion
+		                                         && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning);

 		if (_control_status.flags.fuse_aspd) {
 			if (continuing_conditions_passing) {
@@ -118,17 +118,16 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		} else if (starting_conditions_passing) {
 			ECL_INFO("starting airspeed fusion");

-			// If starting wind state estimation, reset the wind states and covariances before fusing any data
-			// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
-			const Vector2f wind_var_xy = getWindVelocityVariance();
+			if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) {
+				resetVelUsingAirspeed(airspeed_sample);

-			if (!_control_status.flags.wind || (wind_var_xy(0) + wind_var_xy(1) > sq(_params.initial_wind_uncertainty))) {
-				// activate the wind states
-				_control_status.flags.wind = true;
-				// reset the wind speed states and corresponding covariances
+			} else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) {
+				// If starting wind state estimation, reset the wind states and covariances before fusing any data
+				// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
 				resetWindUsingAirspeed(airspeed_sample);
 			}

+			_control_status.flags.wind = true;
 			_control_status.flags.fuse_aspd = true;
 		}

@@ -208,7 +207,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}

-	const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation);
+	const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);

 	aid_src.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;
@@ -247,3 +246,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)

 	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
 }
+
+void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample)
+{
+	const float euler_yaw = getEulerYaw(_R_to_earth);
+
+	// Estimate velocity using zero sideslip assumption and airspeed measurement
+	Vector2f horizontal_velocity;
+	horizontal_velocity(0) = _state.wind_vel(0) + airspeed_sample.true_airspeed * cosf(euler_yaw);
+	horizontal_velocity(1) = _state.wind_vel(1) + airspeed_sample.true_airspeed * sinf(euler_yaw);
+
+	float vel_var = NAN; // Do not reset the velocity variance as wind variance estimate is most likely not correct
+	resetHorizontalVelocityTo(horizontal_velocity, vel_var);
+
+	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
+}
@@ -42,10 +42,10 @@ void Ekf::controlAuxVelFusion()

 			resetEstimatorAidStatus(_aid_src_aux_vel);

-			updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
+			updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);

 			if (isHorizontalAidingActive()) {
-				fuseVelocity(_aid_src_aux_vel);
+				fuseHorizontalVelocity(_aid_src_aux_vel);
 			}
 		}
 	}
@@ -364,7 +364,6 @@ struct parameters {
 	int32_t mag_declination_source{7};      ///< bitmask used to control the handling of declination data
 	int32_t mag_fusion_type{0};             ///< integer used to specify the type of magnetometer fusion used
 	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
-	float mag_yaw_rate_gate{0.20f};         ///< yaw rate threshold used by mode select logic (rad/sec)

 	// compute synthetic magnetomter Z value if possible
 	int32_t synthesize_mag_z{0};
@@ -509,15 +508,9 @@ union fault_status_u {
 		bool bad_sideslip      : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 		bool bad_optflow_X     : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
 		bool bad_optflow_Y     : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
-		bool bad_vel_N         : 1; ///< 9 - true if fusion of the North velocity has encountered a numerical error
-		bool bad_vel_E         : 1; ///< 10 - true if fusion of the East velocity has encountered a numerical error
-		bool bad_vel_D         : 1; ///< 11 - true if fusion of the Down velocity has encountered a numerical error
-		bool bad_pos_N         : 1; ///< 12 - true if fusion of the North position has encountered a numerical error
-		bool bad_pos_E         : 1; ///< 13 - true if fusion of the East position has encountered a numerical error
-		bool bad_pos_D         : 1; ///< 14 - true if fusion of the Down position has encountered a numerical error
-		bool bad_acc_bias      : 1; ///< 15 - true if bad delta velocity bias estimates have been detected
-		bool bad_acc_vertical  : 1; ///< 16 - true if bad vertical accelerometer data has been detected
-		bool bad_acc_clipping  : 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
+		bool bad_acc_bias      : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
+		bool bad_acc_vertical  : 1; ///< 10 - true if bad vertical accelerometer data has been detected
+		bool bad_acc_clipping  : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
 	} flags;
 	uint32_t value;
 };
@@ -222,14 +222,6 @@ void Ekf::constrainStateVariances()
 #endif // CONFIG_EKF2_WIND
 }

-void Ekf::forceCovarianceSymmetry()
-{
-	// DEBUG
-	// P.isBlockSymmetric(0, 1e-9f);
-
-	P.makeRowColSymmetric<State::size>(0);
-}
-
 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
 {
 	for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
@@ -256,22 +248,6 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max,
 	}
 }

-// if the covariance correction will result in a negative variance, then
-// the covariance matrix is unhealthy and must be corrected
-bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
-{
-	bool healthy = true;
-
-	for (int i = 0; i < State::size; i++) {
-		if (P(i, i) < KHP(i, i)) {
-			P.uncorrelateCovarianceSetVariance<1>(i, 0.f);
-			healthy = false;
-		}
-	}
-
-	return healthy;
-}
-
 void Ekf::resetQuatCov(const float yaw_noise)
 {
 	const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));
@@ -160,7 +160,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)

 			VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index];

-			if (measurementUpdate(K, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) {
+			if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) {
 				fused[axis_index] = true;
 			}
 		}
@@ -326,14 +326,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	const float alpha = 1.0f - imu_delayed.delta_vel_dt;
 	_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();

-	// calculate a yaw change about the earth frame vertical
-	const float spin_del_ang_D = corrected_delta_ang.dot(Vector3f(_R_to_earth.row(2)));
-	_yaw_delta_ef += spin_del_ang_D;
-
-	// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
-	// Note fixed coefficients are used to save operations. The exact time constant is not important.
-	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
-
 	// Calculate low pass filtered height rate
 	float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
@@ -327,8 +327,8 @@ public:
 #endif
 	}

-	// fuse single velocity and position measurement
-	bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index);
+	// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
+	bool fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index);

 	// gyro bias
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
@@ -468,35 +468,39 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL

-	bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
+	bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation)
 	{
 		clearInhibitedStateKalmanGains(K);

-		const VectorState KS = K * innovation_variance;
-		SquareMatrixState KHP;
+		// Efficient implementation of the Joseph stabilized covariance update
+		// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"

-		for (unsigned row = 0; row < State::size; row++) {
-			for (unsigned col = 0; col < State::size; col++) {
-				// Instad of literally computing KHP, use an equvalent
-				// equation involving less mathematical operations
-				KHP(row, col) = KS(row) * K(col);
+		// Step 1: conventional update
+		VectorState PH = P * H;
+
+		for (unsigned i = 0; i < State::size; i++) {
+			for (unsigned j = 0; j <= i; j++) {
+				P(i, j) = P(i, j) - K(i) * PH(j);
+				P(j, i) = P(i, j);
 			}
 		}

-		const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
+		// Step 2: stabilized update
+		PH = P * H;

-		if (is_healthy) {
-			// apply the covariance corrections
-			P -= KHP;
-
-			constrainStateVariances();
-			forceCovarianceSymmetry();
-
-			// apply the state corrections
-			fuse(K, innovation);
+		for (unsigned i = 0; i < State::size; i++) {
+			for (unsigned j = 0; j <= i; j++) {
+				float s = .5f * (P(i, j) - PH(i) * K(j) + P(i, j) - PH(j) * K(i));
+				P(i, j) = s + K(i) * R * K(j);
+				P(j, i) = P(i, j);
+			}
 		}

-		return is_healthy;
+		constrainStateVariances();
+
+		// apply the state corrections
+		fuse(K, innovation);
+		return true;
 	}

 	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
@@ -572,8 +576,6 @@ private:

 	Vector2f _accel_lpf_NE{};			///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
 	float _height_rate_lpf{0.0f};
-	float _yaw_delta_ef{0.0f};		///< Recent change in yaw angle measured about the earth frame D axis (rad)
-	float _yaw_rate_lpf_ef{0.0f};		///< Filtered angular rate about earth frame D axis (rad/sec)

 	SquareMatrixState P{};	///< state covariance matrix

@@ -702,9 +704,7 @@ private:
 	float _mag_heading_pred_prev{};            ///< previous value of yaw state used by mag heading fusion (rad)

 	// used by magnetometer fusion mode selection
-	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
 	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
-	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
 	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
 	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
 	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
@@ -720,7 +720,6 @@ private:

 	// Variables used to control activation of post takeoff functionality
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
-	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	uint64_t _time_last_mag_check_failing{0};
 #endif // CONFIG_EKF2_MAGNETOMETER

@@ -784,6 +783,7 @@ private:

 	// Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
 	void resetWindUsingAirspeed(const airspeedSample &airspeed_sample);
+	void resetVelUsingAirspeed(const airspeedSample &airspeed_sample);
 #endif // CONFIG_EKF2_AIRSPEED

 #if defined(CONFIG_EKF2_SIDESLIP)
@@ -828,7 +828,7 @@ private:
 	void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;

 	// 2d & 3d velocity aid source
-	void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
+	void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
 	void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const;

 	// horizontal and vertical position fusion
@@ -836,7 +836,7 @@ private:
 	void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);

 	// 2d & 3d velocity fusion
-	void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
+	void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
 	void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);

 #if defined(CONFIG_EKF2_TERRAIN)
@@ -942,15 +942,9 @@ private:
 #endif // CONFIG_EKF2_WIND
 	}

-	// if the covariance correction will result in a negative variance, then
-	// the covariance matrix is unhealthy and must be corrected
-	bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP);
-
 	// limit the diagonal of the covariance matrix
 	void constrainStateVariances();

-	void forceCovarianceSymmetry();
-
 	void constrainStateVar(const IdxDof &state, float min, float max);
 	void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f);

@@ -1049,7 +1043,6 @@ private:
 	bool haglYawResetReq();

 	void checkYawAngleObservability();
-	void checkMagBiasObservability();
 	void checkMagHeadingConsistency();

 	bool checkMagField(const Vector3f &mag);
@@ -1137,8 +1130,6 @@ private:
 	void resetFakePosFusion();
 	void stopFakePosFusion();

-	void setVelPosStatus(const int state_index, const bool healthy);
-
 	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
 	// yaw : Euler yaw angle (rad)
 	// yaw_variance : yaw error variance (rad^2)
@@ -45,121 +45,6 @@
 #include <lib/world_magnetic_model/geo_mag_declination.h>
 #include <cstdlib>

-void Ekf::resetHorizontalVelocityToZero()
-{
-	_information_events.flags.reset_vel_to_zero = true;
-	ECL_INFO("reset velocity to zero");
-	// Used when falling back to non-aiding mode of operation
-	resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
-}
-
-void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
-{
-	resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
-	resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
-}
-
-void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
-{
-	const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
-	_state.vel.xy() = new_horz_vel;
-
-	if (PX4_ISFINITE(new_horz_vel_var(0))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
-	}
-
-	if (PX4_ISFINITE(new_horz_vel_var(1))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
-	}
-
-	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
-
-	// record the state change
-	if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
-		_state_reset_status.velNE_change = delta_horz_vel;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.velNE_change += delta_horz_vel;
-	}
-
-	_state_reset_status.reset_count.velNE++;
-
-	// Reset the timout timer
-	_time_last_hor_vel_fuse = _time_delayed_us;
-}
-
-void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
-{
-	const float delta_vert_vel = new_vert_vel - _state.vel(2);
-	_state.vel(2) = new_vert_vel;
-
-	if (PX4_ISFINITE(new_vert_vel_var)) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
-	}
-
-	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
-
-	// record the state change
-	if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
-		_state_reset_status.velD_change = delta_vert_vel;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.velD_change += delta_vert_vel;
-	}
-
-	_state_reset_status.reset_count.velD++;
-
-	// Reset the timout timer
-	_time_last_ver_vel_fuse = _time_delayed_us;
-}
-
-void Ekf::resetHorizontalPositionToLastKnown()
-{
-	ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
-
-	_information_events.flags.reset_pos_to_last_known = true;
-
-	// Used when falling back to non-aiding mode of operation
-	resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
-}
-
-void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
-{
-	const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
-	_state.pos.xy() = new_horz_pos;
-
-	if (PX4_ISFINITE(new_horz_pos_var(0))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
-	}
-
-	if (PX4_ISFINITE(new_horz_pos_var(1))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
-	}
-
-	_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
-
-	// record the state change
-	if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
-		_state_reset_status.posNE_change = delta_horz_pos;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.posNE_change += delta_horz_pos;
-	}
-
-	_state_reset_status.reset_count.posNE++;
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-	//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
-
-	// Reset the timout timer
-	_time_last_hor_pos_fuse = _time_delayed_us;
-}
-
 bool Ekf::isHeightResetRequired() const
 {
 	// check if height is continuously failing because of accel errors
@@ -171,68 +56,6 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }

-void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) {
-    _information_events.flags.reset_pos_to_ext_obs = true;
-    ECL_INFO("reset position to external observation");
-    resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
-}
-
-void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
-{
-	const float old_vert_pos = _state.pos(2);
-	_state.pos(2) = new_vert_pos;
-
-	if (PX4_ISFINITE(new_vert_pos_var)) {
-		// the state variance is the same as the observation
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
-	}
-
-	const float delta_z = new_vert_pos - old_vert_pos;
-
-	// apply the change in height / height rate to our newest height / height rate estimate
-	// which have already been taken out from the output buffer
-	_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
-
-	// record the state change
-	if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
-		_state_reset_status.posD_change = delta_z;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.posD_change += delta_z;
-	}
-
-	_state_reset_status.reset_count.posD++;
-
-#if defined(CONFIG_EKF2_BAROMETER)
-	_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_BAROMETER
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_GNSS)
-	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_GNSS
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_TERRAIN)
-	terrainHandleVerticalPositionReset(delta_z);
-#endif
-
-	// Reset the timout timer
-	_time_last_hgt_fuse = _time_delayed_us;
-}
-
-void Ekf::resetVerticalVelocityToZero()
-{
-	// we don't know what the vertical velocity is, so set it to zero
-	// Set the variance to a value large enough to allow the state to converge quickly
-	// that does not destabilise the filter
-	resetVerticalVelocityTo(0.0f, 10.f);
-}
-
 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
 float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
 {
@@ -684,7 +507,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
 	ekf_solution_status_u soln_status{};
 	// TODO: Is this accurate enough?
-	soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
+	soln_status.flags.attitude = attitude_valid();
 	soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
 	soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
@@ -1015,3 +838,45 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 		_accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
 	}
 }
+
+bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index)
+{
+	VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
+
+	// calculate kalman gain K = PHS, where S = 1/innovation variance
+	for (int row = 0; row < State::size; row++) {
+		K(row) = P(row, state_index) / innov_var;
+	}
+
+	clearInhibitedStateKalmanGains(K);
+
+	// Efficient implementation of the Joseph stabilized covariance update
+	// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
+
+	// Step 1: conventional update
+	VectorState PH = P.row(state_index);
+
+	for (unsigned i = 0; i < State::size; i++) {
+		for (unsigned j = 0; j <= i; j++) {
+			P(i, j) = P(i, j) - K(i) * PH(j);
+			P(j, i) = P(i, j);
+		}
+	}
+
+	// Step 2: stabilized update
+	PH = P.row(state_index);
+
+	for (unsigned i = 0; i < State::size; i++) {
+		for (unsigned j = 0; j <= i; j++) {
+			float s = .5f * (P(i, j) - PH(i) * K(j) + P(i, j) - PH(j) * K(i));
+			P(i, j) = s + K(i) * R * K(j);
+			P(j, i) = P(i, j);
+		}
+	}
+
+	constrainStateVariances();
+
+	// apply the state corrections
+	fuse(K, innov);
+	return true;
+}
@@ -41,6 +41,7 @@
 void Ekf::controlExternalVisionFusion()
 {
 	_ev_pos_b_est.predict(_dt_ekf_avg);
+	_ev_hgt_b_est.predict(_dt_ekf_avg);

 	// Check for new external vision data
 	extVisionSample ev_sample;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Eric Katzfey	cc48676eaa	Implemented flush for Serial UART API	2024-03-07 10:27:56 -08:00
Eric Katzfey	65a73496e7	Removed commented out code	2024-03-06 16:53:11 -08:00
Eric Katzfey	bf5bce8718	Fixed processing bug on Qurt platform due to compiler bug	2024-03-06 16:52:20 -08:00
Eric Katzfey	129b11424c	First version working with new Serial API on voxl2	2024-03-06 16:30:59 -08:00
Eric Katzfey	313003d2ac	Made fixes for Qurt platform build and merge issues	2024-03-06 08:51:29 -08:00
Eric Katzfey	635d4557f7	Merge branch 'main' into wip_new_uart_api	2024-03-05 08:56:53 -08:00
Peter van der Perk	d1ae242a91	v6x-rt: fix rover build regression	2024-03-05 10:21:41 -05:00
Peter van der Perk	9cef834624	fmu-v6xrt: update px4board enables vtol	2024-03-05 08:18:44 -05:00
Sihyun Noh	23a41299fa	mag calibration: minor cleanup (#22830 )	2024-03-05 09:00:08 +01:00
Niklas Hauser	0186d687b2	Add minimal Skynode RC13 config to the PAB manifest	2024-03-04 13:33:20 -05:00
Peter van der Perk	d28653b605	nuttx: update apps	2024-03-04 13:32:36 -05:00
Peter van der Perk	87d79aeb75	netman: generate default config if file doesn't exist ENOENT returns if the file doesn't exist yet, when using mtd /fs/mtd_net always exist. On a filesystem you've to generate the file so if ENOENT returns we've to regenerate the default config as well.	2024-03-04 13:32:36 -05:00
Matthias Grob	1bd65f8beb	mantis-edu: remove duplicate RC define	2024-03-04 13:30:28 -05:00
Peter van der Perk	e0b49afe81	bmp388: Driver print out start BMP390 identifier when detected	2024-03-04 02:05:37 -05:00
Don Gagne	f02b44bec5	Update to latest sitl gazebo camera	2024-03-03 12:18:02 +13:00
bresch	28db3e1c8c	ekf2: update change indicator	2024-02-27 12:33:43 -05:00
bresch	e9d43015ce	ekf2: fix unit tests failing due to mag fusion changes	2024-02-27 12:33:43 -05:00
bresch	b46fc9a67d	ekf2 sensor_sim: set correct world mag field	2024-02-27 12:33:43 -05:00
bresch	b80f15f7b5	ekf2-mag_auto: always use mag 3D after takeoff	2024-02-27 12:33:43 -05:00
Silvan Fuhrer	086656dc7f	FW Attitude Controller: fix manual yaw rate setpoint limit (#22812 ) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-27 17:23:13 +01:00
bresch	051baec9c4	ekf2: allow wind dead-reckoning after manual position reset Reset velocity using airspeed and start navigating	2024-02-27 09:34:05 -05:00
Matthias Grob	2491548a0f	Jenkinsfile: correct typo, missing comma Introduced in `2c81c9fdea`	2024-02-27 13:32:46 +01:00
DanielePettenuzzo	18f96c16ce	fix gimbal driver for mavlink gimbal v2 input and AUX output The main problem was that during initial negotiation the client would request the gimbal_manager_information from px4 but px4 would never send it because in this configuration the device_compid was set to 0.	2024-02-27 09:53:25 +01:00
GuillaumeLaine	63495ddac3	geo: correct unit test	2024-02-27 09:05:39 +01:00
Peter van der Perk	efbbd64ec0	fmu-v6xrt: Increase lpwork stack size	2024-02-26 14:01:56 -05:00
Daniel Agar	8001132d33	ekf2: ZeroGyroUpdate move to fuseDirectStateMeasurement	2024-02-26 12:32:59 -05:00
bresch	08a2a6c836	update EKF2 change indicator	2024-02-26 12:32:59 -05:00
bresch	d501d8e1d4	ekf2: use Joseph stabilized update in direct state observations	2024-02-26 12:32:59 -05:00
bresch	9d9766c6cf	ekf2: use Joseph stabilized covariance update	2024-02-26 12:32:59 -05:00
makekam	d988005216	Update injectxmlparams.py Add extraction of Boolean attributes in injectxmlparams.py.	2024-02-23 11:06:05 -05:00
Silvan Fuhrer	5dfdf8c071	matrix: remove bold printing of diagonal elements As this was not working in NSH. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-23 11:05:09 -05:00
Daniel Agar	b2b7439060	ROMFS: respect kconfig for including romfs files (airframes, etc) (#22571 ) * ROMFS: respect kconfig for including romfs files (airframes, etc) * ROMFS: only add R1 airframe with differential drive control * ROMFS: adapt to differential drive module renaming --------- Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-02-23 15:40:00 +01:00
bresch	37a40d3fc2	baro static pressure compensation tuning: remove dependency to baro bias `estimator_baro_bias` requires to have GNSS and baro hgt active and GNSS as the reference. This is quite restrictive. Instead, we can simply use a high-passed version of the baro error.	2024-02-23 10:07:13 +01:00
Sergei Grichine	b405d75553	Added Zero Turn Lawnmower model (#22717 ) * Added Lawnmower airframe * Update 5005_gz_lawnmower Works all right * Update 5005_gz_lawnmower RDD_WHL_SPEED has new name: RDD_WHEEL_SPEED * Update ROMFS/px4fmu_common/init.d-posix/airframes/5005_gz_lawnmower Co-authored-by: Per Frivik <94360401+PerFrivik@users.noreply.github.com> * Update ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt Co-authored-by: Per Frivik <94360401+PerFrivik@users.noreply.github.com> * Renamed 5005_gz_lawnmower to 4011_gz_lawnmower also pulled latest GZ models hash --------- Co-authored-by: Per Frivik <94360401+PerFrivik@users.noreply.github.com>	2024-02-22 15:30:12 +01:00
Matthias Grob	4e3bd4f196	MAVSDK tests: shorten Position, Altitude control flights We get more than 5 meter away much quicker.	2024-02-22 15:29:13 +01:00
Matthias Grob	0cc4b41a51	MAVSDK test: Fix fly_forward_in_altctl() timing	2024-02-22 15:29:13 +01:00
Matthias Grob	f602228048	MAVSDK test: increase offboard position threshold This is a workaround to hotfix CI but the root cause is #22792 (MAVSDK test failing after EKF change, accelerometer simulation issues not learned anymore?)	2024-02-22 15:29:13 +01:00
PerFrivik	9b122adae4	Fix fly_forward_in_posctl() timing	2024-02-22 15:29:13 +01:00
Eric Katzfey	1ec0ba4736	Added param system command to voxl2 slpi build	2024-02-21 11:54:02 -05:00
Eric Katzfey	8da8b88a54	Fixed and added Qurt platform dsp_hitl driver	2024-02-21 11:54:02 -05:00
Eric Katzfey	be08c57a0a	Changed order of service startup in SLPI DSP muorb since parameter library now needs work queues	2024-02-21 11:54:02 -05:00
Eric Katzfey	a436a8f3b8	Fixed unresolved symbol error for qurt platform due to missing sbus library	2024-02-21 11:54:02 -05:00
Eric Katzfey	5ad0e68d8e	Fix build error for Qurt platform in pab_manifest.c	2024-02-21 11:54:02 -05:00
Eric Katzfey	f07eeaa776	Added special muorb startup ordering in px4_init for posix platform	2024-02-21 11:52:24 -05:00
Daniel Agar	506c60c471	ekf2: declination fusion don't use uninitialized parameter (EKF2_MAG_DECL)	2024-02-21 09:45:44 -05:00
Niklas Hauser	643d3e3bf3	Navigator: Prevent busy-looping if Dataman read/write times out MissionBase did not initialize its mission data, thus could enter an infinite loop in updateDatamanCache() if the initMission() failed to read the mission off, for example, due to the SDCard storage task taking longer than the timeout to respond. This change constrains the loading loop and resets the mission data even if the data write failed.	2024-02-21 13:31:43 +01:00
Daniel Agar	8243b4f474	ekf2: move vel/pos reset helpers	2024-02-20 13:16:24 -05:00
Daniel Agar	22b957696d	ekf2: velocity/position fusion helper minor consistency cleanup	2024-02-20 13:16:24 -05:00
Daniel Agar	c338891677	ekf2: split vel_pos_fusion.cpp	2024-02-20 13:16:24 -05:00
Daniel Agar	c4c41c49e5	ekf2: move fuseVelPosHeight() -> fuseDirectStateMeasurement() - don't bother keeping bad_vel_{N,E,D} and bad_pos_{N,E,D} fault status bits	2024-02-20 13:16:24 -05:00
Daniel Agar	021dd0d0af	ekf2: fix EV height bias predict call - needs to be called every iteration	2024-02-20 11:47:53 -05:00
bresch	c221da27a7	ekf2: set attitude validity flag using centralized function	2024-02-20 11:33:30 -05:00
Matthias Grob	51fe4351c6	StickTiltXY: Fix too high maximum tilt problem And add unit tests.	2024-02-20 14:27:49 +01:00
Silvan Fuhrer	8a75733511	Navigator: fix VTOL land waypoint calculation The setpoint.yaw can be NAN, and this made the calculated land point NAN as well. Looking at the current yaw is anyway a better way to approximate the course over ground that fundamentally should be used. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-19 14:54:28 +01:00
Daniel Agar	1032dd3470	ekf: fix measurementUpdate comment typo	2024-02-19 09:41:49 +01:00
Konrad	424c3cd2cb	FeasibilityChecker: Add new TakeoffLandAvailable option ADd a new misison feasiblity checker option to check if a proper landing approach is defined when in air. There must be at least a mission landing or a VTOL approach defined in order for the mission to be accepted. Else, use the same logic as in MIS_TKO_LAND_REQ=4	2024-02-16 10:27:22 +01:00
Konrad	68100650da	RTL: publish a status message on currently chosen RTL point	2024-02-16 10:27:22 +01:00
Eric Katzfey	ff133660c0	Fixed the fmu_v5_protected build failure	2024-02-15 18:32:21 -08:00
Cyril C	74303a79e1	drivers/batt_smbus: fix BQ40Z80 timeout problem (#22751 ) Co-authored-by: cyril.calvez <c.calvez@elistair.com>	2024-02-15 13:24:40 -05:00
Daniel Agar	8dc3975456	ekf2: only populate gnss pos aid src status if ref initialized - this is a minor logging improvement when plotting the position from the beginning of the log (often a replay session)	2024-02-15 13:13:10 -05:00
Matthias Grob	84a7d42566	rover build: correct differential drive kconfig name	2024-02-15 10:08:51 -05:00
Matthias Grob	f26df8492f	Update GPS drivers to contain the astyle fix	2024-02-15 15:23:06 +01:00
Konrad	cb09dde606	FixedwingPositionControl: Used corrected npfg roll output in path mode	2024-02-13 17:17:44 +01:00
Konrad	1a1891073e	FixedwingPositionControl: Only warn user when roll is reduced for a longer period of time	2024-02-13 17:17:44 +01:00
Daniel Agar	b8714f8980	ROMFS: rc.simulator EKF2 setup specific to gazebo classic	2024-02-13 11:14:44 -05:00
PX4 BuildBot	0c099f2b56	Update submodule gz to latest Tue Feb 13 12:39:17 UTC 2024 - gz in PX4/Firmware (`c9ad60e3cc`): https://github.com/PX4/PX4-gazebo-models/commit/c78f7f01417168e8faab7a83ade2129c0d26b39d - gz current upstream: https://github.com/PX4/PX4-gazebo-models/commit/f1c461fffb8567d6f0af770fb533f60f6ec62c22 - Changes: https://github.com/PX4/PX4-gazebo-models/compare/c78f7f01417168e8faab7a83ade2129c0d26b39d...f1c461fffb8567d6f0af770fb533f60f6ec62c22 f1c461f 2024-02-08 frederik - increase monocam clipping distance 6d5db73 2024-02-07 Sergei Grichine - Added Zero Turn Lawnmower model (#27) 5332071 2024-02-06 Frederik Markus - add navsat plugin to worlds and navsat sensor to models (#26)	2024-02-13 11:13:17 -05:00
Frederik Markus	bb53781b8f	simulation/gz_bridge: enable navsat plugin for accurate positioning of real life maps in Gazebo (#22638 ) * publish the global groundtruth from the navsat callback and rearrange the local groundtruth as the altitude reference now has a dependency on the global groundtruth being initialized --------- Signed-off-by: frederik <frederik.anilmarkus@gmail.com>	2024-02-13 11:09:35 -05:00
Silvan Fuhrer	c9ad60e3cc	Update src/modules/navigator/mission_block.cpp Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-02-13 10:34:57 +01:00
Silvan Fuhrer	a6ef7b6da9	RTL: write out weather vane in comments (instead of WV) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-13 10:34:57 +01:00
Silvan Fuhrer	6957818603	RTL: clean up naming of function arguments Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-13 10:34:57 +01:00
Matthias Grob	cb03835124	RTL: use dest.yaw instead of a separate heading_sp	2024-02-13 10:34:57 +01:00
Silvan Fuhrer	b19e35ec7c	RTL: change when to set a heading setpoint, generally leave it up to the executer -remove RTL_HDG_MD -only set heading setpoint in Navigator::RTL once above landing point, or when RTL is triggered close to it -never set a heading during RTL if weather vane is enabled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-13 10:34:57 +01:00
PX4 BuildBot	dce53a626e	boards: update all NuttX defconfigs	2024-02-12 08:58:49 -05:00
Daniel Agar	5f589bdda3	Update submodule GPSDrivers to latest Mon Feb 12 12:39:19 UTC 2024 - GPSDrivers in PX4/Firmware (17ff40898c683e1fe96ff9e2d2790594d188f872): https://github.com/PX4/PX4-GPSDrivers/commit/3393191fbb842f8e13a3f296218efec832640112 - GPSDrivers current upstream: https://github.com/PX4/PX4-GPSDrivers/commit/f48cc01d31607baa4963bde090f530b44df3de12 - Changes: https://github.com/PX4/PX4-GPSDrivers/compare/3393191fbb842f8e13a3f296218efec832640112...f48cc01d31607baa4963bde090f530b44df3de12 f48cc01 2024-02-08 Julian Oes - ubx: separate config for jamming monitor bc72f55 2024-02-08 Julian Oes - sbf: simplify odd define Co-authored-by: PX4 BuildBot <bot@px4.io>	2024-02-12 08:58:21 -05:00
Matthias Grob	1998f54ea6	DifferentialDrive: move spoolup consideration to the main module	2024-02-12 14:29:10 +01:00
PerFrivik	bef694f9ba	Added spoolup and removed temporary timeout for EKF	2024-02-12 14:29:10 +01:00
PerFrivik	560d6a9d4b	cleanup + updated acro	2024-02-12 14:29:10 +01:00
PerFrivik	f996caa5bd	Fixed bug in the guidance logic After smoothing the linera velocity setpoint, the EKF has trouble initializing, becuase the acceleration is too smooth, to combat this issue, there is a 1 second delay when initializing the mission mode	2024-02-12 14:29:10 +01:00
PerFrivik	bb0dfba4e6	added acro mode Acro mode is manual mode, but with rate control	2024-02-12 14:29:10 +01:00
PerFrivik	d197d94889	Fixed guidance logic and added feedforward term to compute the angular velocity	2024-02-12 14:29:10 +01:00
Matthias Grob	396ef222ee	DifferentialDrive: Rework structure 3 Components Guidance - Control - Allocation with their corresponding uORB interface.	2024-02-12 14:29:10 +01:00
Matthias Grob	f85144ca76	DifferentialDrive: remove trailing zeros from prameter metadata	2024-02-12 14:29:10 +01:00
Matthias Grob	b54b4f7dce	Rename module differential_drive_control -> differential_drive	2024-02-12 14:29:10 +01:00
Matthias Grob	fc90e235f1	Rename differential drive setpoint topics	2024-02-12 14:29:10 +01:00
Matthias Grob	f7baeae1a0	DifferentialDriveControl: only save required parts of uORB message	2024-02-12 14:29:10 +01:00
PerFrivik	e457a5baed	Differential Drive Guidance: Add guidance also add dependency on control allocation parameter CA_R_REV Differential Drive Guidance: Added mission logic Differential Drive Guidance Differential Drive Guidance Differential Guidance: Inlcude library Differential Guidance: Compiles, does not work though Differential Guidance: Works somewhat Differential Guidance: Temp Differential Guidance: Tuning Differeital Drive Guidance: Remove waypoint mover Differential Guidance: Fixed accuracy issue by converting from float to double Differential Guidance: rebased on differentialdrive and improved waypoint accuracy Temp Differential Guidance: cleanup temp	2024-02-12 14:29:10 +01:00
Eric Katzfey	42547a1319	Commented out SIH simulation for modalai_fc-v2 build to fit into flash	2024-01-05 14:21:15 -08:00
Eric Katzfey	b2ee342fd6	Merge branch 'main' of github.com:PX4/PX4-Autopilot into wip_new_uart_api	2024-01-04 11:03:46 -08:00
Eric Katzfey	5b0013ebec	Merge branch 'wip_new_uart_api' of github.com:PX4/PX4-Autopilot into wip_new_uart_api	2023-12-27 09:11:40 -08:00
Eric Katzfey	564c37d696	Fixed delete and non-null pointer bug	2023-12-27 09:09:30 -08:00
Eric Katzfey	f6318c4926	More cleanup and fixes from testing failures	2023-12-27 09:09:30 -08:00
Eric Katzfey	231f072dc0	Added getPort back in to Serial interface. Only setPort needed to be removed	2023-12-27 09:09:30 -08:00
Eric Katzfey	88611f2228	Fixed formatting	2023-12-27 09:09:30 -08:00
Eric Katzfey	5938831e50	Added baudrate validation functions to the Serial implementations	2023-12-27 09:09:30 -08:00
Eric Katzfey	ad0ce0b7ed	Adding some copyright notices and general cleanup	2023-12-27 09:09:30 -08:00
Eric Katzfey	9accf81299	Changed the default baudrate handling around a little to make it cleaner	2023-12-27 09:09:30 -08:00
Eric Katzfey	2dbae8488e	Fixed Clang tidy errors	2023-12-27 09:09:30 -08:00
Eric Katzfey	53281e5840	Fixed linking error in SLPI build	2023-12-27 09:09:30 -08:00
Eric Katzfey	820570a5b1	Fixed another GPS SPI option to make it only for Linux builds	2023-12-27 09:09:30 -08:00
Eric Katzfey	9edd3b8532	Fixed formatting and made SPI a Linux only choice to GPS	2023-12-27 09:09:30 -08:00
Eric Katzfey	5d80d23668	First working version. Tested on ModalAI FlightCore V2	2023-12-27 09:09:30 -08:00
Eric Katzfey	936d7c4f28	Fixed delete and non-null pointer bug	2023-07-26 14:15:44 -07:00
Eric Katzfey	81ab314a4c	More cleanup and fixes from testing failures	2023-07-20 15:43:07 -07:00
Eric Katzfey	9c5b1477d4	Added getPort back in to Serial interface. Only setPort needed to be removed	2023-07-20 13:21:27 -07:00
Eric Katzfey	884763b577	Fixed formatting	2023-07-20 13:14:28 -07:00
Eric Katzfey	4672106699	Added baudrate validation functions to the Serial implementations	2023-07-20 13:11:35 -07:00
Eric Katzfey	1b92db9d89	Adding some copyright notices and general cleanup	2023-07-20 10:39:30 -07:00
Eric Katzfey	14a3df8b60	Changed the default baudrate handling around a little to make it cleaner	2023-07-20 10:39:30 -07:00
Eric Katzfey	83f3bc4dff	Fixed Clang tidy errors	2023-07-20 10:39:30 -07:00
Eric Katzfey	8b43a8a5f6	Fixed linking error in SLPI build	2023-07-20 10:39:30 -07:00
Eric Katzfey	960c2ab797	Fixed another GPS SPI option to make it only for Linux builds	2023-07-20 10:39:30 -07:00
Eric Katzfey	4c5e05de13	Fixed formatting and made SPI a Linux only choice to GPS	2023-07-20 10:39:30 -07:00
Eric Katzfey	96c05ac23a	First working version. Tested on ModalAI FlightCore V2	2023-07-20 10:39:30 -07:00