Vector2: Add function to rotate a 2D vector

v2_default: disable hover thrust estimator to safe flash
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2026-06-09 17:10:04 +08:00 · 2023-03-03 19:03:00 +01:00 · 2023-03-02 12:22:18 -05:00 · 2023-03-02 09:52:53 -05:00 · 2023-03-02 12:06:40 +01:00 · 2023-03-02 12:06:40 +01:00
160 changed files with 5164 additions and 2473 deletions
@@ -110,6 +110,8 @@ pipeline {
                     "raspberrypi_pico_default",
                     "sky-drones_smartap-airlink_default",
                     "spracing_h7extreme_default",
+                     "thepeach_k1_default",
+                     "thepeach_r1_default",
                     "uvify_core_default"
            ],
            image: docker_images.nuttx,
@@ -44,81 +44,88 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con

 ## Maintenance Team

-  * Project: Founder
-    * [Lorenz Meier](https://github.com/LorenzMeier)
-  * Architecture
-    * [Daniel Agar](https://github.com/dagar)
-  * [Dev Call](https://github.com/PX4/PX4-Autopilot/labels/devcall)
-    * [Ramon Roche](https://github.com/mrpollo)
-  * Communication Architecture
-    * [Beat Kueng](https://github.com/bkueng)
-    * [Julian Oes](https://github.com/JulianOes)
-  * UI in QGroundControl
-    * [Gus Grubba](https://github.com/dogmaphobic)
-  * [Multicopter Flight Control](https://github.com/PX4/PX4-Autopilot/labels/multicopter)
-    * [Mathieu Bresciani](https://github.com/bresch)
-  * [Multicopter Software Architecture](https://github.com/PX4/PX4-Autopilot/labels/multicopter)
-    * [Matthias Grob](https://github.com/MaEtUgR)
-  * [VTOL Flight Control](https://github.com/PX4/PX4-Autopilot/labels/vtol)
-    * [Roman Bapst](https://github.com/RomanBapst)
-  * [Fixed Wing Flight Control](https://github.com/PX4/PX4-Autopilot/labels/fixedwing)
-    * [Roman Bapst](https://github.com/RomanBapst)
-  * OS / NuttX
-    * [David Sidrane](https://github.com/davids5)
-  * Driver Architecture
-    * [Daniel Agar](https://github.com/dagar)
-  * Commander Architecture
-    * [Julian Oes](https://github.com/julianoes)
-  * [UAVCAN](https://github.com/PX4/PX4-Autopilot/labels/uavcan)
-    * [Daniel Agar](https://github.com/dagar)
-  * [State Estimation](https://github.com/PX4/PX4-Autopilot/issues?q=is%3Aopen+is%3Aissue+label%3A%22state+estimation%22)
-    * [Paul Riseborough](https://github.com/priseborough)
-  * Vision based navigation and Obstacle Avoidance
-    * [Markus Achtelik](https://github.com/markusachtelik)
-  * DDS/ROS2 Interface
-    * [Nuno Marques](https://github.com/TSC21)
+Note: This is the source of truth for the active maintainers of PX4 ecosystem.

-See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github).
+| Sector | Maintainer |
+|---|---|
+| Founder | [Lorenz Meier](https://github.com/LorenzMeier) |
+| Architecture | [Daniel Agar](https://github.com/dagar) / [Beat Küng](https://github.com/bkueng)|
+| State Estimation | [Mathieu Bresciani](https://github.com/bresch) / [Paul Riseborough](https://github.com/priseborough) |
+| OS/NuttX | [David Sidrane](https://github.com/davids5) |
+| Drivers | [Daniel Agar](https://github.com/dagar) |
+| Simulation | [Jaeyoung Lim](https://github.com/Jaeyoung-Lim) |
+| ROS2 | [Beniamino Pozzan](https://github.com/beniaminopozzan) |
+| Community QnA Call | [Ramon Roche](https://github.com/mrpollo) |
+| [Documentation](https://docs.px4.io/main/en/) | [Hamish Willee](https://github.com/hamishwillee) |
+
+| Vehicle Type | Maintainer |
+|---|---|
+| Multirotor | [Matthias Grob](https://github.com/MaEtUgR) |
+| Fixed Wing | [Thomas Stastny](https://github.com/tstastny) |
+| Hybrid VTOL | [Silvan Fuhrer](https://github.com/sfuhrer) |
+| Boat | x |
+| Rover | x |
+
+See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). However it may be not up to date.

 ## Supported Hardware

-This repository contains code supporting Pixhawk standard boards (best supported, best tested, recommended choice) and proprietary boards.
+Pixhawk standard boards and proprietary boards are shown below (discontinued boards aren't listed).
+
+For the most up to date information, please visit [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/).

 ### Pixhawk Standard Boards
-  * FMUv6X and FMUv6U (STM32H7, 2021)
-    * Various vendors will provide FMUv6X and FMUv6U based designs Q3/2021
-  * FMUv5 and FMUv5X (STM32F7, 2019/20)
-    * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html)
-    * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html)
-    * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html)
-    * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html)
-    * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/skynode)
-  * FMUv4 (STM32F4, 2015)
-    * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html)
-    * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html)
-  * FMUv3 (STM32F4, 2014)
-    * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html)
-    * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html)
-    * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html)
-  * FMUv2 (STM32F4, 2013)
-    * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html)
-    * [Pixfalcon](https://docs.px4.io/main/en/flight_controller/pixfalcon.html)

-### Manufacturer and Community supported
-  * [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
-  * [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
-  * [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
-  * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
-  * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
-  * [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html)
-  * [Omnibus F4 SD](https://docs.px4.io/main/en/flight_controller/omnibus_f4_sd.html)
-  * [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html)
-  * [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html)
+These boards fully comply with Pixhawk Standard, and are maintained by the PX4-Autopilot maintainers and Dronecode team

-Additional information about supported hardware can be found in [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/).
+* FMUv6X and FMUv6C
+  * [CUAV Pixahwk V6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/cuav_pixhawk_v6x.html)
+  * [Holybro Pixhawk 6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/pixhawk6x.html)
+  * [Holybro Pixhawk 6C (FMUv6C)](https://docs.px4.io/main/en/flight_controller/pixhawk6c.html)
+  * [Holybro Pix32 v6 (FMUv6C)](https://docs.px4.io/main/en/flight_controller/holybro_pix32_v6.html)
+* FMUv5 and FMUv5X (STM32F7, 2019/20)
+  * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html)
+  * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html)
+  * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html)
+  * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html)
+  * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/avionics/skynode)
+* FMUv4 (STM32F4, 2015)
+  * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html)
+  * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html)
+* FMUv3 (STM32F4, 2014)
+  * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html)
+  * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html)
+  * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html)
+* FMUv2 (STM32F4, 2013)
+  * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html)
+
+### Manufacturer supported
+
+These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers.
+
+* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html)
+* [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
+* [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
+* [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
+* [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
+* [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
+* [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html)
+
+### Community supported
+
+These boards don't fully comply industry standards, and thus is solely maintained by the PX4 publc community members.
+
+### Experimental
+
+These boards are nor maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases.
+
+* [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html)
+* [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html)

 ## Project Roadmap

+**Note: Outdated**
+
 A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25).

 ## Project Governance
@@ -12,6 +12,10 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadx}

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+
 # disable some checks to allow to fly:
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -11,6 +11,11 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=airplane}

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
 # disable some checks to allow to fly:
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -11,6 +11,10 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
@@ -13,6 +13,10 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500}

 param set-default SIM_GZ_EN 1

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+
 param set-default CA_AIRFRAME 0
 param set-default CA_ROTOR_COUNT 4

@@ -13,6 +13,10 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_depth}

 param set-default SIM_GZ_EN 1

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+
 param set-default CA_AIRFRAME 0
 param set-default CA_ROTOR_COUNT 4

@@ -10,6 +10,9 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
 PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna}

+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

 param set-default EKF2_MAG_ACCLIM 0
@@ -11,6 +11,9 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
 PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}

+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

 # TODO: Enable motor failure detection when the
@@ -10,9 +10,18 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"

 	if simulator_sih start; then

-		sensor_baro_sim start
-		sensor_gps_sim start
-		sensor_mag_sim start
+		if param compare -s SENS_EN_BAROSIM 1
+		then
+			sensor_baro_sim start
+		fi
+		if param compare -s SENS_EN_GPSSIM 1
+		then
+			sensor_gps_sim start
+		fi
+		if param compare -s SENS_EN_MAGSIM 1
+		then
+			sensor_mag_sim start
+		fi

 	else
 		echo "ERROR [init] simulator_sih failed to start"
@@ -77,9 +86,18 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th

 		# start gz bridge with pose arg.
 		if gz_bridge start -p "${model_pose}" -m "${PX4_GZ_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
-			sensor_baro_sim start
-			sensor_gps_sim start
-			sensor_mag_sim start
+			if param compare -s SENS_EN_BAROSIM 1
+			then
+				sensor_baro_sim start
+			fi
+			if param compare -s SENS_EN_GPSSIM 1
+			then
+				sensor_gps_sim start
+			fi
+			if param compare -s SENS_EN_MAGSIM 1
+			then
+				sensor_mag_sim start
+			fi
 			if param compare -s SENS_EN_ARSPDSIM 1
 			then
 				sensor_airspeed_sim start
@@ -94,9 +112,18 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 		# model name specificed, gz_bridge will attach to existing model

 		if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
-			sensor_baro_sim start
-			sensor_gps_sim start
-			sensor_mag_sim start
+			if param compare -s SENS_EN_BAROSIM 1
+			then
+				sensor_baro_sim start
+			fi
+			if param compare -s SENS_EN_GPSSIM 1
+			then
+				sensor_gps_sim start
+			fi
+			if param compare -s SENS_EN_MAGSIM 1
+			then
+				sensor_mag_sim start
+			fi
 			if param compare -s SENS_EN_ARSPDSIM 1
 			then
 				sensor_airspeed_sim start
@@ -112,9 +139,18 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 		echo "WARN  [init] PX4_GZ_MODEL_NAME or PX4_GZ_MODEL not set using PX4_SIM_MODEL."

 		if gz_bridge start -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
-			sensor_baro_sim start
-			sensor_gps_sim start
-			sensor_mag_sim start
+			if param compare -s SENS_EN_BAROSIM 1
+			then
+				sensor_baro_sim start
+			fi
+			if param compare -s SENS_EN_GPSSIM 1
+			then
+				sensor_gps_sim start
+			fi
+			if param compare -s SENS_EN_MAGSIM 1
+			then
+				sensor_mag_sim start
+			fi
 			if param compare -s SENS_EN_ARSPDSIM 1
 			then
 				sensor_airspeed_sim start
@@ -36,6 +36,7 @@ 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
@@ -17,7 +17,6 @@ param set-default COM_POS_FS_DELAY 5

 # there is a 2.5 factor applied on the _FS thresholds if for invalidation
 param set-default COM_POS_FS_EPH 50
-param set-default COM_POS_FS_EPV 30
 param set-default COM_VEL_FS_EVH 5

 param set-default COM_POS_LOW_EPH 50
@@ -25,7 +25,7 @@ if __name__ == "__main__":
    if not os.path.isdir(output_dir):
        os.mkdir(output_dir)

-    msg_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"..")
+    msg_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"../../msg")
    msg_files = get_msgs_list(msg_path)
    msg_files.sort()

@@ -164,7 +164,8 @@ for field_type, field_name, field_size, padding in fields:
 	print('\tmemcpy(&topic.{0}, buf.iterator, sizeof(topic.{0}));'.format(field_name))

 	if field_type == 'uint64' and (field_name == 'timestamp' or field_name == 'timestamp_sample'):
-		print('\ttopic.{0} -= time_offset;'.format(field_name))
+		print('\tif (topic.{0} == 0) topic.{0} = hrt_absolute_time();'.format(field_name, field_name))
+		print('\telse topic.{0} = math::min(topic.{0} - time_offset, hrt_absolute_time());'.format(field_name, field_name))

 	print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name))
 	print('\tbuf.offset += sizeof(topic.{:});'.format(field_name))
@@ -84,6 +84,7 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i
 	gdb \
 	git \
 	lcov \
+	libfuse2 \
 	libxml2-dev \
 	libxml2-utils \
 	make \
@@ -182,6 +183,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 			echo "${NUTTX_GCC_VERSION} path already set.";
 		else
 			echo $exportline >> $HOME/.profile;
+			source $HOME/.profile; # Allows to directly build NuttX targets in the same terminal
 		fi
 	fi
 fi
@@ -217,33 +219,34 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	# Set Java 11 as default
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")

-	# Install Gazebo
+	# Gazebo / Gazebo classic installation
 	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		echo "Gazebo (Garden) will be installed"
+		echo "Earlier versions will be removed"
+		# Add Gazebo binary repository
+		sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+		echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+		sudo apt-get update -y --quiet
+
+		# Install Gazebo
+		gazebo_packages="gz-garden"
+	else
 		sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
 		wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 		# Update list, since new gazebo-stable.list has been added
 		sudo apt-get update -y --quiet
-		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-			ignition-fortress \
-			;
+
+		# Install Gazebo classic
+		if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
+			gazebo_classic_version=9
+			gazebo_packages="gazebo$gazebo_classic_version libgazebo$gazebo_classic_version-dev"
+		else
+			# default and Ubuntu 20.04
+			gazebo_classic_version=11
+			gazebo_packages="gazebo$gazebo_classic_version libgazebo$gazebo_classic_version-dev"
+		fi
 	fi

-	# Install Gazebo classic
-	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
-		gazebo_version=9
-		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
-	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		gazebo_packages="gazebo libgazebo-dev"
-	else
-		# default and Ubuntu 20.04
-		gazebo_version=11
-		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
-	fi
-
-	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-	# Update list, since new gazebo-stable.list has been added
-	sudo apt-get update -y --quiet
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		dmidecode \
 		$gazebo_packages \
@@ -84,6 +84,18 @@
        <always_on>1</always_on>
        <update_rate>250</update_rate>
      </sensor>
+      <sensor name="air_pressure_sensor" type="air_pressure">
+        <always_on>1</always_on>
+        <update_rate>50</update_rate>
+        <air_pressure>
+          <pressure>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </pressure>
+        </air_pressure>
+      </sensor>
    </link>
    <link name="airspeed">
      <pose>0 0 0 0 0 0</pose>
@@ -134,6 +134,18 @@
        <always_on>1</always_on>
        <update_rate>250</update_rate>
      </sensor>
+      <sensor name="air_pressure_sensor" type="air_pressure">
+        <always_on>1</always_on>
+        <update_rate>50</update_rate>
+        <air_pressure>
+          <pressure>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </pressure>
+        </air_pressure>
+      </sensor>
    </link>
    <link name='rotor_0'>
      <pose>0.35 -0.35 0.07 0 0 0</pose>
@@ -215,6 +215,18 @@
          </friction>
        </surface>
      </collision>
+      <sensor name="air_pressure_sensor" type="air_pressure">
+        <always_on>1</always_on>
+        <update_rate>50</update_rate>
+        <air_pressure>
+          <pressure>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </pressure>
+        </air_pressure>
+      </sensor>
      <sensor name="imu_sensor" type="imu">
        <always_on>1</always_on>
        <update_rate>250</update_rate>
@@ -10,6 +10,7 @@
    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
    <plugin name='ignition::gazebo::systems::Imu' filename='ignition-gazebo-imu-system'/>
+    <plugin name='ignition::gazebo::systems::AirPressure' filename='ignition-gazebo-air-pressure-system'/>
    <plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
      <render_engine>ogre2</render_engine>
    </plugin>
@@ -195,5 +195,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	sdio_mediachange(sdio_dev, true);
 #endif /* CONFIG_MMCSD */

+	px4_platform_configure();
+
 	return OK;
 }
@@ -4,6 +4,8 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_COMMON_LIGHT=y
+CONFIG_CYPHAL_BMS_SUBSCRIBER=y
+CONFIG_DRIVERS_CYPHAL=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
@@ -15,6 +15,7 @@ param set-default MAV_1_REMOTE_PRT 14550
 param set-default MAV_1_UDP_PRT 14550

 param set-default SENS_EXT_I2C_PRB 0
+param set-default CYPHAL_ENABLE 0

 if param greater -s UAVCAN_ENABLE 0
 then
@@ -22,4 +23,11 @@ then
 	ifup can1
 	ifup can2
 	ifup can3
-fi
+fi
+if param greater -s CYPHAL_ENABLE 0
+then
+	ifup can0
+	ifup can1
+	ifup can2
+	ifup can3
+fi
@@ -29,7 +29,7 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
@@ -4,3 +4,4 @@ CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
 CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
 CONFIG_DRIVERS_CYPHAL=y
+CONFIG_CYPHAL_ESC_CONTROLLER=y
@@ -47,6 +47,7 @@ then
 	fi
 fi

+
 if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004
 then
 	# Internal SPI bus ICM20649
@@ -101,7 +102,13 @@ if ver hwtypecmp V6X002001
 then
 	rm3100 -I -b 4 start
 else
-	bmm150 -I -R 6 start
+	if ver hwtypecmp V6X009010 V6X010010
+	then
+		# Internal magnetometer on I2C
+		bmm150 -I -R 0 start
+	else
+		bmm150 -I -R 6 start
+	fi
 fi

 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
@@ -28,7 +28,6 @@ bool local_position_invalid           # Local position estimate invalid
 bool local_position_invalid_relaxed   # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
 bool local_velocity_invalid           # Local velocity estimate invalid
 bool global_position_invalid          # Global position estimate invalid
-bool gps_position_invalid             # GPS position invalid
 bool auto_mission_missing             # No mission available
 bool offboard_control_signal_lost     # Offboard signal lost
 bool home_position_invalid            # No home position available
@@ -28,8 +28,3 @@ float32 throttle_trim			# estimated throttle value [0,1] required to fly level a
 uint8 mode
 uint8 TECS_MODE_NORMAL = 0
 uint8 TECS_MODE_UNDERSPEED = 1
-uint8 TECS_MODE_TAKEOFF = 2
-uint8 TECS_MODE_LAND = 3
-uint8 TECS_MODE_LAND_THROTTLELIM = 4
-uint8 TECS_MODE_BAD_DESCENT = 5
-uint8 TECS_MODE_CLIMBOUT = 6
@@ -76,18 +76,18 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};

 static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19};

-static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21};
-static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22};
-static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23};
-static constexpr wq_config_t ttyS3{"wq:ttyS3", 1632, -24};
-static constexpr wq_config_t ttyS4{"wq:ttyS4", 1632, -25};
-static constexpr wq_config_t ttyS5{"wq:ttyS5", 1632, -26};
-static constexpr wq_config_t ttyS6{"wq:ttyS6", 1632, -27};
-static constexpr wq_config_t ttyS7{"wq:ttyS7", 1632, -28};
-static constexpr wq_config_t ttyS8{"wq:ttyS8", 1632, -29};
-static constexpr wq_config_t ttyS9{"wq:ttyS9", 1632, -30};
-static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1632, -31};
-static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1632, -32};
+static constexpr wq_config_t ttyS0{"wq:ttyS0", 1728, -21};
+static constexpr wq_config_t ttyS1{"wq:ttyS1", 1728, -22};
+static constexpr wq_config_t ttyS2{"wq:ttyS2", 1728, -23};
+static constexpr wq_config_t ttyS3{"wq:ttyS3", 1728, -24};
+static constexpr wq_config_t ttyS4{"wq:ttyS4", 1728, -25};
+static constexpr wq_config_t ttyS5{"wq:ttyS5", 1728, -26};
+static constexpr wq_config_t ttyS6{"wq:ttyS6", 1728, -27};
+static constexpr wq_config_t ttyS7{"wq:ttyS7", 1728, -28};
+static constexpr wq_config_t ttyS8{"wq:ttyS8", 1728, -29};
+static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30};
+static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31};
+static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32};

 static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};

@@ -124,6 +124,13 @@ typedef struct {
 // wait for the sensor hub if its data is coming from it.
 #define SCHED_PRIORITY_ESTIMATOR		(PX4_WQ_HP_BASE - 5)

+// Logger watchdog priority, triggered when a task busy-loops (and
+// restored after a short time).
+// The priority is a trade-off between:
+// - ability to capture any busy-looping task below this priority
+// - not having a negative impact on the system itself
+#define SCHED_PRIORITY_LOG_WATCHDOG		(PX4_WQ_HP_BASE - 6)
+
 // Position controllers typically are in a blocking wait on estimator data
 // so when new sensor data is available they will run last. Keeping them
 // on a high priority ensures that they are the first process to be run
@@ -53,6 +53,7 @@
 #include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_test.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
@@ -69,69 +70,38 @@ public:
 	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;

 	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
-		UavcanPublisher(handle, pmgr, "udral", "esc") { };
+		UavcanPublisher(handle, pmgr, "udral.", "esc") { };

 	~UavcanEscController() {};

 	void update() override
 	{
+		actuator_test_s actuator_test;
+
+		if (_actuator_test_sub.update(&actuator_test)) {
+			_actuator_test_timestamp = actuator_test.timestamp;
+		}
+
 		if (_armed_sub.updated()) {
 			actuator_armed_s new_arming;
 			_armed_sub.update(&new_arming);

 			if (new_arming.armed != _armed.armed) {
 				_armed = new_arming;
-				size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
-
-				// Only publish if we have a valid publication ID set
-				if (_port_id == 0) {
-					return;
-				}
-
-				reg_udral_service_common_Readiness_0_1 msg_arming {};
-
-				if (_armed.armed) {
-					msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
-
-				} else if (_armed.prearmed) {
-					msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
-
-				} else {
-					msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
-				}
-
-				uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
-
-				CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
-				const CanardTransferMetadata transfer_metadata = {
-					.priority       = CanardPriorityNominal,
-					.transfer_kind  = CanardTransferKindMessage,
-					.port_id        = arming_pid,                // This is the subject-ID.
-					.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
-					.transfer_id    = _arming_transfer_id,
-				};
-
-				int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
-						&payload_size);
-
-				if (result == 0) {
-					// set the data ready in the buffer and chop if needed
-					++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-					result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-								       &transfer_metadata,
-								       payload_size,
-								       &arming_payload_buffer
-								      );
-				}
+				publish_readiness();
 			}
 		}
+
+		if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
+			publish_readiness();
+		}
 	};

 	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
 	{
 		if (_port_id > 0) {
 			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
-			size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+			size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;

 			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
 				if (i < num_outputs) {
@@ -143,10 +113,6 @@ public:
 				}
 			}

-
-			PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1],
-				 (double)msg_sp.value[2], (double)msg_sp.value[3]);
-
 			uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];

 			const CanardTransferMetadata transfer_metadata = {
@@ -182,10 +148,64 @@ private:
 	 */
 	void esc_status_sub_cb(const CanardRxTransfer &msg);

+	void publish_readiness()
+	{
+		const hrt_abstime now = hrt_absolute_time();
+		_previous_pub_time = now;
+
+		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+		// Only publish if we have a valid publication ID set
+		if (_port_id == 0) {
+			return;
+		}
+
+		reg_udral_service_common_Readiness_0_1 msg_arming {};
+
+		if (_armed.armed || _actuator_test_timestamp + 100_ms > now) {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
+
+		} else if (_armed.prearmed) {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
+
+		} else {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
+		}
+
+		uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = arming_pid,                // This is the subject-ID.
+			.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = _arming_transfer_id,
+		};
+
+		int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
+				&payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						       &transfer_metadata,
+						       payload_size,
+						       &arming_payload_buffer);
+		}
+	};
+
 	uint8_t _rotor_count {0};

+	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
+	hrt_abstime _previous_pub_time = 0;
+
 	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
 	actuator_armed_s _armed {};

+	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	uint64_t _actuator_test_timestamp{0};
+
 	CanardTransferID _arming_transfer_id;
 };
@@ -131,7 +131,7 @@ void CanardHandle::receive()

 		} else if (result == 1) {
 			// A transfer has been received, process it.
-			// PX4_INFO("received Port ID: %d", receive.port_id);
+			// PX4_INFO("received Port ID: %d", receive.metadata.port_id);

 			if (subscription != nullptr) {
 				UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
@@ -145,7 +145,7 @@ void CanardHandle::receive()
 			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);

 		} else {
-			//PX4_INFO("RX canard %d", result);
+			// PX4_INFO("RX canard %li\n", result);
 		}
 	}

@@ -83,6 +83,8 @@ CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) :
 	_pub_manager.updateParams();

 	_sub_manager.subscribe();
+
+	_mixing_output.mixingOutput().setMaxTopicUpdateRate(1000000 / 200);
 }

 CyphalNode::~CyphalNode()
@@ -135,6 +137,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
 	_instance->active_bitrate = bitrate;

 	_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
+	_instance->_mixing_output.ScheduleNow();

 	return PX4_OK;
 }
@@ -254,17 +257,19 @@ void CyphalNode::print_info()

 	_pub_manager.printInfo();

+	PX4_INFO("Message subscriptions:");
 	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindMessage),
 	[&](const CanardRxSubscription * const sub) {
 		if (sub->user_reference == nullptr) {
 			PX4_INFO("Message port id %d", sub->port_id);

 		} else {
-			((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
+			((UavcanBaseSubscriber *)sub->user_reference)->printInfo(sub->port_id);
 		}
 	});


+	PX4_INFO("Service response subscriptions:");
 	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindRequest),
 	[&](const CanardRxSubscription * const sub) {
 		if (sub->user_reference == nullptr) {
@@ -275,6 +280,7 @@ void CyphalNode::print_info()
 		}
 	});

+	PX4_INFO("Service request subscriptions:");
 	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindResponse),
 	[&](const CanardRxSubscription * const sub) {
 		if (sub->user_reference == nullptr) {
@@ -393,8 +399,11 @@ bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX
 	// Hence, the mutex lock in UavcanMixingInterface::Run() is in effect

 	/// TODO: Need esc/servo metadata / bitmask(s)
-	_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
-	// _servo_controller.update_outputs(stop_motors, outputs, num_outputs);
+	auto publisher = static_cast<UavcanEscController *>(_pub_manager.getPublisher("esc"));
+
+	if (publisher) {
+		publisher->update_outputs(stop_motors, outputs, num_outputs);
+	}

 	return true;
 }
@@ -82,11 +82,10 @@ class UavcanMixingInterface : public OutputModuleInterface
 {
 public:
 	UavcanMixingInterface(pthread_mutex_t &node_mutex,
-			      UavcanEscController &esc_controller) //, UavcanServoController &servo_controller)
+			      PublicationManager &pub_manager)
 		: OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan),
 		  _node_mutex(node_mutex),
-		  _esc_controller(esc_controller)/*,
-		  _servo_controller(servo_controller)*/ {}
+		  _pub_manager(pub_manager) {}

 	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
@@ -103,8 +102,7 @@ protected:
 private:
 	friend class CyphalNode;
 	pthread_mutex_t &_node_mutex;
-	UavcanEscController &_esc_controller;
-	// UavcanServoController &_servo_controller;
+	PublicationManager &_pub_manager;
 	MixingOutput _mixing_output{"UCAN1_ESC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 };

@@ -115,7 +113,7 @@ class CyphalNode : public ModuleParams, public px4::ScheduledWorkItem
 	 * Base interval, has to be complemented with events from the CAN driver
 	 * and uORB topics sending data, to decrease response time.
 	 */
-	static constexpr unsigned ScheduleIntervalMs = 10;
+	static constexpr unsigned ScheduleIntervalMs = 3;

 public:

@@ -178,9 +176,6 @@ private:
 	PublicationManager _pub_manager {_canard_handle, _param_manager};
 	SubscriptionManager _sub_manager {_canard_handle, _param_manager};

-	/// TODO: Integrate with PublicationManager
-	UavcanEscController _esc_controller {_canard_handle, _param_manager};
-
-	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
+	UavcanMixingInterface _mixing_output {_node_mutex, _pub_manager};

 };
@@ -117,19 +117,19 @@ private:


 	const UavcanParamBinder _uavcan_params[13] {
-		{"uavcan.pub.esc.0.id",                "UCAN1_ESC_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.servo.0.id",              "UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.gps.0.id",                "UCAN1_GPS_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.actuator_outputs.0.id",   "UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.esc.0.id",                "UCAN1_ESC0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.gps.0.id",                "UCAN1_GPS0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.gps.1.id",                "UCAN1_GPS1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.energy_source.0.id",      "UCAN1_BMS_ES_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.esc.0.id",                	"UCAN1_ESC_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.servo.0.id",              	"UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.gps.0.id",                	"UCAN1_GPS_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.actuator_outputs.0.id",   	"UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.esc.0.id",                	"UCAN1_ESC0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.gps.0.id",                	"UCAN1_GPS0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.gps.1.id",                	"UCAN1_GPS1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.energy_source.0.id",      	"UCAN1_BMS_ES_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.battery_status.0.id",     	"UCAN1_BMS_BS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.battery_parameters.0.id",	"UCAN1_BMS_BP_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.legacy_bms.0.id",         	"UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};
@@ -114,6 +114,18 @@ void PublicationManager::updateParams()
 	updateDynamicPublications();
 }

+UavcanPublisher *PublicationManager::getPublisher(const char *subject_name)
+{
+	for (auto &dynpub : _dynpublishers) {
+		if (strcmp(dynpub->getSubjectName(), subject_name) == 0) {
+			return dynpub;
+		}
+	}
+
+	return NULL;
+}
+
+
 void PublicationManager::update()
 {
 	for (auto &dynpub : _dynpublishers) {
@@ -101,6 +101,8 @@ public:
 	void printInfo();
 	void updateParams();

+	UavcanPublisher *getPublisher(const char *subject_name);
+
 private:
 	void updateDynamicPublications();

@@ -116,7 +118,7 @@ private:
 			{
 				return new UavcanGnssPublisher(handle, pmgr, 0);
 			},
-			"gps",
+			"udral.gps",
 			0
 		},
 #endif
@@ -126,7 +128,7 @@ private:
 			{
 				return new UavcanEscController(handle, pmgr);
 			},
-			"esc",
+			"udral.esc",
 			0
 		},
 #endif
@@ -136,7 +138,7 @@ private:
 			{
 				return new UavcanReadinessPublisher(handle, pmgr, 0);
 			},
-			"readiness",
+			"udral.readiness",
 			0
 		},
 #endif
@@ -122,18 +122,26 @@ public:
 		return _subj_sub._subject_name;
 	}

+	const char *getSubjectPrefix()
+	{
+		return _prefix_name;
+	}
+
 	uint8_t getInstance()
 	{
 		return _instance;
 	}

-	void printInfo()
+	void printInfo(CanardPortID port_id = CANARD_PORT_ID_UNSET)
 	{
 		SubjectSubscription *curSubj = &_subj_sub;

 		while (curSubj != nullptr) {
 			if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
-				PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
+				if (port_id == CANARD_PORT_ID_UNSET ||
+				    port_id == curSubj->_canard_sub.port_id) {
+					PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
+				}
 			}

 			curSubj = curSubj->next;
@@ -61,6 +61,8 @@ public:
 	void updateParam()
 	{
 		SubjectSubscription *curSubj = &_subj_sub;
+		bool unsubscribeRequired = false;
+		bool subscribeRequired = false;

 		while (curSubj != nullptr) {
 			char uavcan_param[90];
@@ -76,29 +78,37 @@ public:
 				if (curSubj->_canard_sub.port_id != new_id) {
 					if (new_id == CANARD_PORT_ID_UNSET) {
 						// Cancel subscription
-						unsubscribe();
+						unsubscribeRequired = true;

 					} else {
 						if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
 							// Already active; unsubscribe first
-							unsubscribe();
+							unsubscribeRequired = true;
 						}

 						// Subscribe on the new port ID
 						curSubj->_canard_sub.port_id = (CanardPortID)new_id;
 						PX4_INFO("Subscribing %s%s.%d on port %d", _prefix_name, curSubj->_subject_name, _instance,
 							 curSubj->_canard_sub.port_id);
-						subscribe();
+						subscribeRequired = true;
 					}
 				}

 			} else if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { // No valid sub id unsubscribe when neccesary
 				// Already active; unsubscribe first
-				unsubscribe();
+				unsubscribeRequired = true;
 			}

 			curSubj = curSubj->next;
 		}
+
+		if (unsubscribeRequired) {
+			unsubscribe();
+		}
+
+		if (subscribeRequired) {
+			subscribe();
+		}
 	};

 	UavcanDynamicPortSubscriber *next()
@@ -110,7 +110,7 @@ public:
 			bat_status.connected = true; // Based on some threshold or an error??

 			// Estimate discharged mah from Joule
-			if (_nominal_voltage != NAN) {
+			if (PX4_ISFINITE(_nominal_voltage)) {
 				bat_status.discharged_mah = (source_ts.value.full_energy.joule - source_ts.value.energy.joule)
 							    / (_nominal_voltage * WH_TO_JOULE);
 			}
@@ -78,10 +78,13 @@ void SubscriptionManager::updateDynamicSubscriptions()

 		while (dynsub != nullptr) {
 			// Check if subscriber has already been created
+			const char *subj_prefix = dynsub->getSubjectPrefix();
 			const char *subj_name = dynsub->getSubjectName();
 			const uint8_t instance = dynsub->getInstance();
+			char subject_name[90];
+			snprintf(subject_name, sizeof(subject_name), "%s%s", subj_prefix, subj_name);

-			if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) {
+			if (strcmp(subject_name, sub.subject_name) == 0 && instance == sub.instance) {
 				found_subscriber = true;
 				break;
 			}
@@ -126,7 +126,7 @@ private:
 			{
 				return new UavcanEscSubscriber(handle, pmgr, 0);
 			},
-			"esc",
+			"udral.esc",
 			0
 		},
 #endif
@@ -136,7 +136,7 @@ private:
 			{
 				return new UavcanGnssSubscriber(handle, pmgr, 0);
 			},
-			"gps",
+			"udral.gps",
 			0
 		},
 #endif
@@ -146,7 +146,7 @@ private:
 			{
 				return new UavcanGnssSubscriber(handle, pmgr, 1);
 			},
-			"gps",
+			"udral.gps",
 			1
 		},
 #endif
@@ -156,7 +156,7 @@ private:
 			{
 				return new UavcanBmsSubscriber(handle, pmgr, 0);
 			},
-			"energy_source",
+			"udral.energy_source",
 			0
 		},
 #endif
@@ -166,7 +166,7 @@ private:
 			{
 				return new UavcanLegacyBatteryInfoSubscriber(handle, pmgr, 0);
 			},
-			"legacy_bms",
+			"udral.legacy_bms",
 			0
 		},
 #endif
@@ -0,0 +1,50 @@
+############################################################################
+#
+#   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# 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_module(
+	MODULE drivers__distance_sensor__lightware_sf45_serial
+	MAIN lightware_sf45_serial
+	COMPILE_FLAGS
+	SRCS
+		lightware_sf45_serial.cpp
+		lightware_sf45_serial.hpp
+		lightware_sf45_serial_main.cpp
+	DEPENDS
+		drivers_rangefinder
+		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
+	)
+
+if(PX4_TESTING)
+	add_subdirectory(tests)
+endif()
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_SF45_SERIAL
+	bool "lightware_sf45_serial"
+	default n
+	---help---
+		Enable support for lightware_sf45_serial
@@ -0,0 +1,761 @@
+/**************************************************************************
+ *
+ *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 "lightware_sf45_serial.hpp"
+#include "sf45_commands.h"
+
+#include <inttypes.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <lib/systemlib/crc.h>
+
+#include <float.h>
+#include <mathlib/mathlib.h>
+#include <matrix/matrix/math.hpp>
+
+/* Configuration Constants */
+#define SF45_MAX_PAYLOAD 256
+#define SF45_SCALE_FACTOR 0.01f
+
+SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
+	_px4_rangefinder(0, rotation),
+	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
+{
+	/* store port name */
+	strncpy(_port, port, sizeof(_port) - 1);
+
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
+
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	uint8_t bus_num = atoi(&_port[strlen(_port) - 1]); // Assuming '/dev/ttySx'
+
+	if (bus_num < 10) {
+		device_id.devid_s.bus = bus_num;
+	}
+
+	_num_retries = 2;
+	_px4_rangefinder.set_device_id(device_id.devid);
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
+
+	// populate obstacle map members
+	_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
+	_obstacle_map_msg.increment = 5;
+	_obstacle_map_msg.angle_offset = -2.5;
+	_obstacle_map_msg.min_distance = UINT16_MAX;
+	_obstacle_map_msg.max_distance = 5000;
+
+}
+
+SF45LaserSerial::~SF45LaserSerial()
+{
+	stop();
+
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int SF45LaserSerial::init()
+{
+
+	param_get(param_find("SF45_UPDATE_CFG"), &_update_rate);
+	param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
+	param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
+
+	/* SF45/B (50M) */
+	_px4_rangefinder.set_min_distance(0.2f);
+	_px4_rangefinder.set_max_distance(50.0f);
+	_interval = 10000;
+
+	start();
+
+	return PX4_OK;
+}
+
+int SF45LaserSerial::measure()
+{
+
+	int rate = (int)_update_rate;
+	_data_output = 0x101; // raw distance + yaw readings
+	_stream_data = 5; // enable constant streaming
+
+	// send some packets so the sensor starts scanning
+	switch (_sensor_state) {
+
+	// sensor should now respond
+	case 0:
+		while (_num_retries--) {
+			sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
+			_sensor_state = 0;
+		}
+
+		_sensor_state = 1;
+		break;
+
+	case 1:
+		// Update rate default to 50 readings/s
+		sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t));
+		_sensor_state = 2;
+		break;
+
+	case 2:
+		sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output));
+		_sensor_state = 3;
+		break;
+
+	case 3:
+		sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data));
+		_sensor_state = 4;
+		break;
+
+	default:
+		break;
+	}
+
+	return PX4_OK;
+}
+
+int SF45LaserSerial::collect()
+{
+	perf_begin(_sample_perf);
+
+	/* clear buffer if last read was too long ago */
+	int64_t read_elapsed = hrt_elapsed_time(&_last_read);
+	int ret;
+	/* the buffer for read chars is buflen minus null termination */
+	param_get(param_find("SF45_CP_LIMIT"), &_collision_constraint);
+	uint8_t readbuf[SF45_MAX_PAYLOAD];
+
+	float distance_m = -1.0f;
+
+	/* read from the sensor (uart buffer) */
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	if (_sensor_state == 1) {
+
+		ret = ::read(_fd, &readbuf[0], 22);
+		sf45_request_handle(ret, readbuf);
+		ScheduleDelayed(_interval * 2);
+
+	} else if (_sensor_state == 2) {
+
+		ret = ::read(_fd, &readbuf[0], 7);
+
+		if (readbuf[3] == SF_UPDATE_RATE) {
+			sf45_request_handle(ret, readbuf);
+			ScheduleDelayed(_interval * 5);
+		}
+
+	} else if (_sensor_state == 3) {
+
+		ret = ::read(_fd, &readbuf[0], 8);
+
+		if (readbuf[3] == SF_DISTANCE_OUTPUT) {
+			sf45_request_handle(ret, readbuf);
+			ScheduleDelayed(_interval * 5);
+		}
+
+	} else {
+
+		ret = ::read(_fd, &readbuf[0], 10);
+		uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2);
+
+		if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
+
+			for (uint8_t i = 0; i < ret; ++i) {
+				sf45_request_handle(ret, readbuf);
+			}
+
+			if (_init_complete) {
+				sf45_process_replies(&distance_m);
+			} // end if
+
+		} else {
+
+			ret = ::read(_fd, &readbuf[0], 10);
+
+		}
+	}
+
+	if (ret < 0) {
+		PX4_DEBUG("read err: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+
+		/* only throw an error if we time out */
+		if (read_elapsed > (_interval * 2)) {
+			PX4_DEBUG("Timing out...");
+			return ret;
+
+		} else {
+
+			return -EAGAIN;
+		}
+
+	} else if (ret == 0) {
+		return -EAGAIN;
+	}
+
+	_last_read = hrt_absolute_time();
+
+	if (!_crc_valid) {
+		return -EAGAIN;
+	}
+
+	PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO"));
+	_px4_rangefinder.update(timestamp_sample, distance_m);
+
+	perf_end(_sample_perf);
+
+	return PX4_OK;
+}
+
+void SF45LaserSerial::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+
+	/* schedule a cycle to start things */
+	ScheduleNow();
+}
+
+void SF45LaserSerial::stop()
+{
+	ScheduleClear();
+}
+
+void SF45LaserSerial::Run()
+{
+	/* fds initialized? */
+	if (_fd < 0) {
+		/* open fd: non-blocking read mode*/
+
+		_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+		if (_fd < 0) {
+			PX4_ERR("open failed (%i)", errno);
+			return;
+		}
+
+		struct termios uart_config;
+
+		int termios_state;
+
+		/* fill the struct for the new configuration */
+		tcgetattr(_fd, &uart_config);
+
+		uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8;
+
+		uart_config.c_cflag |= (CLOCAL | CREAD);
+
+		// no parity, 1 stop bit, flow control disabled
+		uart_config.c_cflag &= ~(PARENB | PARODD);
+
+		uart_config.c_cflag |= 0;
+
+		uart_config.c_cflag &= ~CSTOPB;
+
+		uart_config.c_cflag &= ~CRTSCTS;
+
+		uart_config.c_iflag &= ~IGNBRK;
+
+		uart_config.c_iflag &= ~ICRNL;
+
+		uart_config.c_iflag &= ~(IXON | IXOFF | IXANY);
+
+		// echo and echo NL off, canonical mode off (raw mode)
+		// extended input processing off, signal chars off
+		uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+		uart_config.c_oflag = 0;
+
+		uart_config.c_cc[VMIN] = 0;
+
+		uart_config.c_cc[VTIME] = 1;
+
+		unsigned speed = B921600;
+
+		/* set baud rate */
+		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d ISPD", termios_state);
+		}
+
+		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d OSPD", termios_state);
+		}
+
+		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
+			PX4_ERR("baud %d ATTR", termios_state);
+		}
+
+	}
+
+	if (_collect_phase) {
+
+		/* perform collection */
+		int collect_ret = collect();
+
+		if (collect_ret == -EAGAIN) {
+			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
+			ScheduleDelayed(1042 * 8);
+
+			return;
+		}
+
+		if (OK != collect_ret) {
+			/* we know the sensor needs about four seconds to initialize */
+			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+				PX4_ERR("collection error #%u", _consecutive_fail_count);
+			}
+
+			_consecutive_fail_count++;
+
+			/* restart the measurement state machine */
+			start();
+			return;
+
+		} else {
+			/* apparently success */
+			_consecutive_fail_count = 0;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+	}
+
+	/* measurement phase */
+
+	if (OK != measure()) {
+		PX4_DEBUG("measure error");
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	ScheduleDelayed(_interval);
+
+}
+
+void SF45LaserSerial::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+}
+
+void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
+{
+
+	// SF45 protocol
+	// Start byte is 0xAA and is the start of packet
+	// Payload length sanity check (0-1023) bytes
+	// and represented by 16-bit integer (payload +
+	// read/write status)
+	// ID byte precedes the data in the payload
+	// CRC comprised of 16-bit checksum (not included in checksum calc.)
+
+	uint16_t recv_crc = 0;
+	bool restart_flag = false;
+
+	while (restart_flag != true) {
+
+		switch (_parsed_state) {
+		case 0: {
+				if (input_buf[0] == 0xAA) {
+					// start of frame is valid, continue
+					_sop_valid = true;
+					_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
+					_parsed_state = 1;
+					break;
+
+				} else {
+					_sop_valid = false;
+					_crc_valid = false;
+					_parsed_state = 0;
+					restart_flag = true;
+					_calc_crc = 0;
+					PX4_INFO("INFO: start of packet not valid");
+					break;
+				} // end else
+			} // end case 0
+
+		case 1: {
+				rx_field.flags_lo = input_buf[1];
+				_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
+				_parsed_state = 2;
+				break;
+			}
+
+		case 2: {
+				rx_field.flags_hi = input_buf[2];
+				rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6);
+				_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi);
+
+				// Check payload length against known max value
+				if (rx_field.data_len > 17) {
+					PX4_INFO("INFO: payload length invalid, restarting data request");
+					_parsed_state = 0;
+					restart_flag = true;
+					_calc_crc = 0;
+					break;
+
+				} else {
+					_parsed_state = 3;
+					break;
+				}
+			}
+
+		case 3: {
+
+				rx_field.msg_id = input_buf[3];
+
+				if (rx_field.msg_id == SF_PRODUCT_NAME || rx_field.msg_id == SF_UPDATE_RATE || rx_field.msg_id == SF_DISTANCE_OUTPUT
+				    || rx_field.msg_id == SF_STREAM || rx_field.msg_id == SF_DISTANCE_DATA_CM) {
+
+					if (rx_field.msg_id == SF_DISTANCE_DATA_CM && rx_field.data_len > 1) {
+						_sensor_ready = true;
+
+					} else {
+						_sensor_ready = false;
+					}
+
+					_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
+
+					_parsed_state = 4;
+					break;
+				}
+
+				// Ignore message ID's that aren't defined
+				else {
+					_parsed_state = 0;
+					_calc_crc = 0;
+					restart_flag = true;
+					break;
+
+				}
+			}
+
+		// Data
+		case 4: {
+				// Process commands with  & w/out data bytes
+				if (rx_field.data_len > 1) {
+					for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) {
+
+						rx_field.data[_data_bytes_recv] = input_buf[i];
+						_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
+						_data_bytes_recv = _data_bytes_recv + 1;
+
+					}  // end for
+				} //end if
+
+				else {
+
+					_parsed_state = 5;
+					_data_bytes_recv = 0;
+					_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
+
+				}
+
+				_parsed_state = 5;
+				_data_bytes_recv = 0;
+				break;
+			}
+
+		// CRC low byte
+		case 5: {
+				rx_field.crc[0] = input_buf[3 + rx_field.data_len];
+				_parsed_state = 6;
+				break;
+			}
+
+		// CRC high byte
+		case 6: {
+				rx_field.crc[1] = input_buf[4 + rx_field.data_len];
+				recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
+
+				// Check the received crc bytes from the sf45 against our own CRC calcuation
+				// If it matches, we can check if sensor ready
+				// Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete
+				if (recv_crc == _calc_crc) {
+					_crc_valid = true;
+
+					// Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM
+					if (_sensor_ready) {
+						_init_complete = true;
+
+					} else {
+						_init_complete = false;
+					}
+
+					_parsed_state = 0;
+					_calc_crc = 0;
+					restart_flag = true;
+					break;
+
+				} else {
+					PX4_INFO("INFO: CRC mismatch");
+					_crc_valid = false;
+					_init_complete = false;
+					_parsed_state = 0;
+					_calc_crc = 0;
+					restart_flag = true;
+					break;
+				}
+			}
+		} // end switch
+	} //end while
+}
+
+void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len)
+{
+	uint16_t crc_val = 0;
+	uint8_t packet_buff[SF45_MAX_PAYLOAD];
+	uint8_t data_inc = 4;
+	int ret = 0;
+	uint8_t packet_len = 0;
+	// Include payload ID byte in payload len
+	uint16_t flags = (data_len + 1) << 6;
+
+	// If writing to the device, lsb is 1
+	if (write) {
+		flags |= 0x01;
+	}
+
+	else {
+		flags |= 0x0;
+	}
+
+	uint8_t flags_lo = flags & 0xFF;
+	uint8_t flags_hi = (flags >> 8) & 0xFF;
+
+	// Add packet bytes to format into crc based on CRC-16-CCITT 0x1021/XMODEM
+	crc_val = sf45_format_crc(crc_val, _start_of_frame);
+	crc_val = sf45_format_crc(crc_val, flags_lo);
+	crc_val = sf45_format_crc(crc_val, flags_hi);
+	crc_val = sf45_format_crc(crc_val, msg_id);
+
+	// Write the packet header contents + payload msg ID to the output buffer
+	packet_buff[0] = _start_of_frame;
+	packet_buff[1] = flags_lo;
+	packet_buff[2] = flags_hi;
+	packet_buff[3] = msg_id;
+
+	if (msg_id == SF_DISTANCE_OUTPUT) {
+		uint8_t data_convert = data[0] & 0x00FF;
+		// write data bytes to the output buffer
+		packet_buff[data_inc] =  data_convert;
+		// Add data bytes to crc add function
+		crc_val = sf45_format_crc(crc_val, data_convert);
+		data_inc = data_inc + 1;
+		data_convert = data[0] >> 8;
+		packet_buff[data_inc] = data_convert;
+		crc_val = sf45_format_crc(crc_val, data_convert);
+		data_inc = data_inc + 1;
+		packet_buff[data_inc] = 0;
+		crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
+		data_inc = data_inc + 1;
+		packet_buff[data_inc] = 0;
+		crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
+		data_inc = data_inc + 1;
+	}
+
+	else if (msg_id == SF_STREAM) {
+		packet_buff[data_inc] = data[0];
+		//pad zeroes
+		crc_val = sf45_format_crc(crc_val, data[0]);
+		data_inc = data_inc + 1;
+		packet_buff[data_inc] = 0;
+		crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
+		data_inc = data_inc + 1;
+		packet_buff[data_inc] = 0;
+		crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
+		data_inc = data_inc + 1;
+		packet_buff[data_inc] = 0;
+		crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
+		data_inc = data_inc + 1;
+	}
+
+	else if (msg_id == SF_UPDATE_RATE) {
+		// Update Rate
+		packet_buff[data_inc] = (uint8_t)data[0];
+		// Add data bytes to crc add function
+		crc_val = sf45_format_crc(crc_val, data[0]);
+		data_inc = data_inc + 1;
+
+	}
+
+	else {
+		// Product Name
+		PX4_INFO("INFO: Product name");
+	}
+
+	uint8_t crc_lo = crc_val & 0xFF;
+	uint8_t crc_hi = (crc_val >> 8) & 0xFF;
+
+	packet_buff[data_inc] = crc_lo;
+	data_inc = data_inc + 1;
+	packet_buff[data_inc] = crc_hi;
+
+	size_t len = sizeof(packet_buff[0]) * (data_inc + 1);
+	packet_len = (uint8_t)len;
+
+	// DEBUG
+	for (uint8_t i = 0; i < packet_len; ++i) {
+		PX4_INFO("INFO: Send byte: %d", packet_buff[i]);
+	}
+
+	ret = ::write(_fd, packet_buff, packet_len);
+
+	if (ret != packet_len) {
+		perf_count(_comms_errors);
+		PX4_DEBUG("write fail %d", ret);
+		//return ret;
+	}
+}
+
+void SF45LaserSerial::sf45_process_replies(float *distance_m)
+{
+
+	switch (rx_field.msg_id) {
+	case SF_DISTANCE_DATA_CM: {
+
+			uint16_t obstacle_dist_cm = 0;
+			const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
+			int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
+			int16_t scaled_yaw = 0;
+
+			// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
+			if (raw_yaw > 32000) {
+				raw_yaw = raw_yaw - 65535;
+
+			}
+
+			// The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle
+			if (_orient_cfg == 1) {
+				raw_yaw = raw_yaw * -1;
+			}
+
+			switch (_yaw_cfg) {
+			case 0:
+				break;
+
+			case 1:
+				if (raw_yaw > 180) {
+					raw_yaw = raw_yaw - 180;
+
+				} else {
+					raw_yaw = raw_yaw + 180; // rotation facing aft
+				}
+
+				break;
+
+			case 2:
+				raw_yaw = raw_yaw + 90; // rotation facing right
+				break;
+
+			case 3:
+				raw_yaw = raw_yaw - 90; // rotation facing left
+				break;
+
+			default:
+				break;
+			}
+
+			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
+
+			// Convert to meters for rangefinder update
+			*distance_m = raw_distance * SF45_SCALE_FACTOR;
+			obstacle_dist_cm = (uint16_t)raw_distance;
+
+			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
+
+			// If we have moved to a new bin
+
+			if (current_bin != _previous_bin) {
+
+				// update the current bin to the distance sensor reading
+				// readings in cm
+				_obstacle_map_msg.distances[current_bin] = obstacle_dist_cm;
+				_obstacle_map_msg.timestamp = hrt_absolute_time();
+
+			}
+
+			_previous_bin = current_bin;
+
+			_obstacle_distance_pub.publish(_obstacle_map_msg);
+
+			break;
+		}
+
+	default:
+		// add case for future use
+		break;
+	}
+}
+
+uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
+{
+
+	uint8_t mapped_sector = 0;
+	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset);
+	mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment);
+
+	return mapped_sector;
+}
+
+float SF45LaserSerial::sf45_wrap_360(float f)
+{
+	return matrix::wrap(f, 0.f, 360.f);
+}
+
+uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val)
+{
+	uint32_t i;
+	const uint16_t poly = 0x1021u;
+	crc ^= (uint16_t)((uint16_t) data_val << 8u);
+
+	for (i = 0; i < 8; i++) {
+		if (crc & (1u << 15u)) {
+			crc = (uint16_t)((crc << 1u) ^ poly);
+
+		} else {
+			crc = (uint16_t)(crc << 1u);
+		}
+	}
+
+	return crc;
+}
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 lightware_sf45_serial.hpp
+ * @author Andrew Brahim <dirksavage88@gmail.com>
+ *
+ * Serial Protocol driver for the Lightware SF45/B rangefinder series
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/device.h>
+#include <lib/parameters/param.h>
+#include <lib/perf/perf_counter.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include "sf45_commands.h"
+class SF45LaserSerial : public px4::ScheduledWorkItem
+{
+public:
+	SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	~SF45LaserSerial() override;
+
+	int 			        init();
+	void				print_info();
+	void                            sf45_request_handle(int val, uint8_t *value);
+	void                            sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len);
+	uint16_t                        sf45_format_crc(uint16_t crc, uint8_t data_value);
+	void                            sf45_process_replies(float *data);
+	uint8_t                         sf45_convert_angle(const int16_t yaw);
+	float                           sf45_wrap_360(float f);
+protected:
+	obstacle_distance_s                       _obstacle_map_msg{};
+	uORB::Publication<obstacle_distance_s>		_obstacle_distance_pub{ORB_ID(obstacle_distance)};	/**< obstacle_distance publication */
+
+private:
+
+	void				start();
+	void				stop();
+	void				Run() override;
+	int				measure();
+	int				collect();
+	bool                            _crc_valid{false};
+	PX4Rangefinder                  _px4_rangefinder;
+
+	char 				_port[20] {};
+	int	        _interval{10000};
+	bool				_collect_phase{false};
+	int 				_fd{-1};
+	int         _linebuf[256] {};
+	unsigned		_linebuf_index{0};
+	hrt_abstime _last_read{0};
+
+	// SF45/B uses a binary protocol to include header,flags
+	// message ID, payload, and checksum
+	bool                            _is_sf45{false};
+	bool                            _init_complete{false};
+	bool                            _sensor_ready{false};
+	uint8_t                         _sensor_state{0};
+	int                             _baud_rate{0};
+	int                             _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+	int                             _stream_data{0};
+	int32_t                         _update_rate{1};
+	int                             _data_output{0};
+	const uint8_t                   _start_of_frame{0xAA};
+	uint16_t                        _data_bytes_recv{0};
+	uint8_t                         _parsed_state{0};
+	bool                            _sop_valid{false};
+	uint16_t                        _calc_crc{0};
+	uint8_t                         _num_retries{2};
+	int32_t                         _yaw_cfg{0};
+	int32_t                         _orient_cfg{0};
+	int32_t                         _collision_constraint{0};
+	uint16_t                        _previous_bin{0};
+
+	// end of SF45/B data members
+
+	unsigned			                  _consecutive_fail_count;
+
+	perf_counter_t			            _sample_perf;
+	perf_counter_t			            _comms_errors;
+
+};
@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 "lightware_sf45_serial.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+namespace lightware_sf45
+{
+
+SF45LaserSerial *g_dev{nullptr};
+
+static int start(const char *port, uint8_t rotation)
+{
+	if (g_dev != nullptr) {
+		PX4_WARN("already started");
+		return -1;
+	}
+
+	if (port == nullptr) {
+		PX4_ERR("no device specified");
+		return -1;
+	}
+
+	/* create the driver */
+	g_dev = new SF45LaserSerial(port, rotation);
+
+	if (g_dev == nullptr) {
+		return -1;
+	}
+
+	if (g_dev->init() != PX4_OK) {
+		delete g_dev;
+		g_dev = nullptr;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		return -1;
+	}
+
+	return 0;
+}
+
+static int status()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return -1;
+	}
+
+	g_dev->print_info();
+
+	return 0;
+}
+
+static int usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+Serial bus driver for the Lightware SF45/b Laser rangefinder.
+
+Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
+
+### Examples
+
+Attempt to start driver on a specified serial device.
+$ lightware_sf45_serial start -d /dev/ttyS1
+Stop driver
+$ lightware_sf45_serial stop
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("lightware_sf45_serial", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
+	return PX4_OK;
+}
+
+} // namespace
+
+extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
+{
+	uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING;
+	const char *device_path = nullptr;
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			break;
+
+		case 'd':
+			device_path = myoptarg;
+			break;
+
+		default:
+			lightware_sf45::usage();
+			return -1;
+		}
+	}
+
+	if (myoptind >= argc) {
+		lightware_sf45::usage();
+		return -1;
+	}
+
+	if (!strcmp(argv[myoptind], "start")) {
+		return lightware_sf45::start(device_path, rotation);
+
+	} else if (!strcmp(argv[myoptind], "stop")) {
+		return lightware_sf45::stop();
+
+	} else if (!strcmp(argv[myoptind], "status")) {
+		return lightware_sf45::status();
+	}
+
+	lightware_sf45::usage();
+	return -1;
+}
@@ -0,0 +1,63 @@
+module_name: Lightware SF45 Rangefinder (serial)
+serial_config:
+    - command: lightware_sf45_serial start -R 0 -d ${SERIAL_DEV}
+      port_config_param:
+        name: SENS_EN_SF45_CFG
+        group: Sensors
+        default: TEL2
+      num_instances: 1
+      supports_networking: false
+
+parameters:
+    - group: Sensors
+      definitions:
+          SF45_UPDATE_CFG:
+              description:
+                  short: Update rate in Hz
+                  long: |
+                      The SF45 sets the update rate in Hz to allow greater resolution
+              type: enum
+              values:
+                  1: 50hz
+                  2: 100hz
+                  3: 200hz
+                  4: 400hz
+                  5: 500hz
+                  6: 625hz
+                  7: 1000hz
+                  8: 1250hz
+                  9: 1538hz
+                  10: 2000hz
+                  11: 2500hz
+                  12: 5000hz
+              reboot_required: true
+              num_instances: 1
+              default: 1
+
+          SF45_ORIENT_CFG:
+              description:
+                  short: Orientation upright or facing downward
+                  long: |
+                      The SF45 mounted facing upward or downward on the frame
+              type: enum
+              values:
+                0: Rotation upward
+                1: Rotation downward
+              reboot_required: true
+              num_instances: 1
+              default: 0
+
+          SF45_YAW_CFG:
+              description:
+                  short: Sensor facing forward or backward
+                  long: |
+                      The usb port on the sensor indicates 180deg, opposite usb is forward facing
+              type: enum
+              values:
+                0: Rotation forward
+                1: Rotation backward
+                2: Rotation right
+                3: Rotation left
+              reboot_required: true
+              num_instances: 1
+              default: 0
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 sf45_commands.h
+ * @author Andrew Brahim
+ *
+ * Declarations of sf45 serial commands for the Lightware sf45/b series
+ */
+
+#pragma once
+#define SF45_MAX_PAYLOAD 256
+#define SF45_CRC_FIELDS 2
+
+enum SF_SERIAL_CMD {
+	SF_PRODUCT_NAME = 0,
+	SF_HARDWARE_VERSION = 1,
+	SF_FIRMWARE_VERSION = 2,
+	SF_SERIAL_NUMBER = 3,
+	SF_TEXT_MESSAGE = 7,
+	SF_USER_DATA = 9,
+	SF_TOKEN = 10,
+	SF_SAVE_PARAMETERS = 12,
+	SF_RESET = 14,
+	SF_STAGE_FIRMWARE = 16,
+	SF_COMMIT_FIRMWARE = 17,
+	SF_DISTANCE_OUTPUT = 27,
+	SF_STREAM = 30,
+	SF_DISTANCE_DATA_CM = 44,
+	SF_DISTANCE_DATA_MM = 45,
+	SF_LASER_FIRING = 50,
+	SF_TEMPERATURE = 57,
+	SF_UPDATE_RATE = 66,
+	SF_NOISE = 74,
+	SF_ZERO_OFFSET = 75,
+	SF_LOST_SIGNAL_COUNTER = 76,
+	SF_BAUD_RATE = 79,
+	SF_I2C_ADDRESS = 80,
+	SF_SCAN_SPEED = 85,
+	SF_STEPPER_STATUS = 93,
+	SF_SCAN_ON_STARTUP = 94,
+	SF_SCAN_ENABLE = 96,
+	SF_SCAN_POSITION = 97,
+	SF_SCAN_LOW_ANGLE = 98,
+	SF_HIGH_ANGLE = 99
+};
+
+// Store contents of rx'd frame
+struct {
+	const uint8_t data_fields = 2; // useful for breaking crc's into byte separated fields
+	uint16_t data_len{0};   // last message payload length (1+ bytes in payload)
+	uint8_t data[SF45_MAX_PAYLOAD];   // payload size limited by posix serial
+	uint8_t msg_id{0};          // latest message's message id
+	uint8_t flags_lo{0};      // flags low byte
+	uint8_t flags_hi{0};     // flags high byte
+	uint16_t crc[SF45_CRC_FIELDS] = {0, 0};
+	uint8_t crc_lo{0};        // crc low byte
+	uint8_t crc_hi{0};       // crc high byte
+} rx_field;
@@ -642,7 +642,7 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 	accel.timestamp_sample = timestamp_sample;
 	accel.samples = 0;

-	// 18-bits of accelerometer data
+	// 18-bits of accelerometer data, sent as 20-bits, 2 least significant bits always 0
 	bool scale_20bit = false;

 	// first pass
@@ -738,7 +738,7 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 	gyro.timestamp_sample = timestamp_sample;
 	gyro.samples = 0;

-	// 20-bits of gyroscope data
+	// 19-bits of gyroscope data sent as 20-bits, LSB bit is 0
 	bool scale_20bit = false;

 	// first pass
@@ -775,6 +775,7 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 			scale_20bit = true;
 		}

+		// shift by 1 (least significant bit is always 0)
 		gyro.x[gyro.samples] = gyro_x / 2;
 		gyro.y[gyro.samples] = gyro_y / 2;
 		gyro.z[gyro.samples] = gyro_z / 2;
@@ -115,7 +115,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 				      GPSGROUP_NONE)
 	   ) {
 		// TIMEGROUP_TIMESTARTUP
-		//uint64_t time_startup = VnUartPacket_extractUint64(packet);
+		uint64_t time_startup = VnUartPacket_extractUint64(packet);
+		(void)time_startup;

 		// IMUGROUP_UNCOMPACCEL
 		vec3f accel = VnUartPacket_extractVec3f(packet);
@@ -141,7 +142,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 				      GPSGROUP_NONE)
 	   ) {
 		// TIMEGROUP_TIMESTARTUP
-		//const uint64_t time_startup = VnUartPacket_extractUint64(packet);
+		const uint64_t time_startup = VnUartPacket_extractUint64(packet);
+		(void)time_startup;

 		// IMUGROUP_TEMP
 		const float temperature = VnUartPacket_extractFloat(packet);
@@ -276,7 +278,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 				      GPSGROUP_NONE)
 	   ) {
 		// TIMEGROUP_TIMESTARTUP
-		//const uint64_t time_startup = VnUartPacket_extractUint64(packet);
+		const uint64_t time_startup = VnUartPacket_extractUint64(packet);
+		(void)time_startup;

 		// GPSGROUP_UTC
 		// TimeUtc timeUtc;
@@ -358,6 +361,8 @@ bool VectorNav::init()

 	if ((VnSensor_connect(&_vs, _port, DEFAULT_BAUDRATE) != E_NONE) || !VnSensor_verifySensorConnectivity(&_vs)) {

+		VnSensor_disconnect(&_vs);
+
 		static constexpr uint32_t BAUDRATES[] {9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};

 		for (auto &baudrate : BAUDRATES) {
@@ -373,11 +378,19 @@ bool VectorNav::init()
 		}
 	}

+	if (!VnSensor_verifySensorConnectivity(&_vs)) {
+		PX4_ERR("Error verifying sensor connectivity");
+		VnSensor_disconnect(&_vs);
+		return false;
+	}
+
 	VnError error = E_NONE;

 	// change baudrate to max
 	if ((error = VnSensor_changeBaudrate(&_vs, DESIRED_BAUDRATE)) != E_NONE) {
-		PX4_WARN("Error changing baud rate failed: %d", error);
+		PX4_ERR("Error changing baud rate failed: %d", error);
+		VnSensor_disconnect(&_vs);
+		return false;
 	}

 	// query the sensor's model number
@@ -385,6 +398,7 @@ bool VectorNav::init()

 	if ((error = VnSensor_readModelNumber(&_vs, model_number, sizeof(model_number))) != E_NONE) {
 		PX4_ERR("Error reading model number %d", error);
+		VnSensor_disconnect(&_vs);
 		return false;
 	}

@@ -393,6 +407,7 @@ bool VectorNav::init()

 	if ((error = VnSensor_readHardwareRevision(&_vs, &hardware_revision)) != E_NONE) {
 		PX4_ERR("Error reading HW revision %d", error);
+		VnSensor_disconnect(&_vs);
 		return false;
 	}

@@ -401,6 +416,7 @@ bool VectorNav::init()

 	if ((error = VnSensor_readSerialNumber(&_vs, &serial_number)) != E_NONE) {
 		PX4_ERR("Error reading serial number %d", error);
+		VnSensor_disconnect(&_vs);
 		return false;
 	}

@@ -409,6 +425,7 @@ bool VectorNav::init()

 	if ((error = VnSensor_readFirmwareVersion(&_vs, firmware_version, sizeof(firmware_version))) != E_NONE) {
 		PX4_ERR("Error reading firmware version %d", error);
+		VnSensor_disconnect(&_vs);
 		return false;
 	}

@@ -456,7 +473,6 @@ bool VectorNav::configure()
 		PX4_ERR("Error reading VPE basic control %d", error);
 	}

-
 	VnError VnSensor_readImuFilteringConfiguration(VnSensor * sensor, ImuFilteringConfigurationRegister * fields);
 	// VnError VnSensor_writeImuFilteringConfiguration(VnSensor *sensor, ImuFilteringConfigurationRegister fields, bool waitForReply);

@@ -479,7 +495,7 @@ bool VectorNav::configure()
 	// binary output 1: max rate IMU
 	BinaryOutputRegister_initialize(
 		&_binary_output_group_1,
-		ASYNCMODE_PORT1,
+		ASYNCMODE_PORT2,
 		1, // divider
 		COMMONGROUP_NONE,
 		TIMEGROUP_TIMESTARTUP,
@@ -502,7 +518,7 @@ bool VectorNav::configure()
 	// binary output 2: medium rate AHRS, INS, baro, mag
 	BinaryOutputRegister_initialize(
 		&_binary_output_group_2,
-		ASYNCMODE_PORT1,
+		ASYNCMODE_PORT2,
 		8, // divider
 		COMMONGROUP_NONE,
 		TIMEGROUP_TIMESTARTUP,
@@ -521,7 +537,7 @@ bool VectorNav::configure()
 	// binary output 3: low rate GNSS
 	BinaryOutputRegister_initialize(
 		&_binary_output_group_3,
-		ASYNCMODE_PORT1,
+		ASYNCMODE_PORT2,
 		80, // divider
 		COMMONGROUP_NONE,
 		TIMEGROUP_TIMESTARTUP,
@@ -577,9 +593,6 @@ void VectorNav::Run()
 		}
 	}

-
-
-
 	ScheduleDelayed(100_ms);
 }

@@ -667,4 +667,4 @@ typedef enum
 	VNPROCESSOR_IMU   /**< IMU Processor. */
 } VnProcessorType;

-#endif
+#endif
@@ -26,9 +26,9 @@
 	#endif
 #endif

-#ifdef __linux__
+#if defined(__linux__)
 	#include <linux/serial.h>
-#elif defined __APPLE__
+#elif defined __APPLE__ || defined(__NUTTX__)
 	#include <dirent.h>
 #endif

@@ -39,7 +39,7 @@ typedef struct
 	#if _WIN32
 	double pcFrequency;
 	__int64 counterStart;
-	#elif __linux__ || __APPLE__ ||__CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ ||__CYGWIN__ || __QNXNTO__ || __NUTTX__
 	double clockStart;
 	#else
 	#error "Unknown System"
@@ -9,7 +9,7 @@
 #include <stdlib.h>


-#ifdef __linux__
+#if defined(__linux__) || defined(__NUTTX__)
 #include <errno.h>
 #include <fcntl.h>
 #include <unistd.h>
@@ -310,7 +310,7 @@ int32_t VnSearcher_getPortBaud(VnPortInfo* portInfo)
 	return portInfo->baud;
 }

-#ifdef __linux__
+#if defined(__linux__) || defined(__NUTTX__)
 void VnSearcher_findPorts_LINUX(char*** portNamesOut, int32_t* numPortsFound)
 {
 	char portName[15] = {0};
@@ -330,9 +330,9 @@ void VnSearcher_findPorts_LINUX(char*** portNamesOut, int32_t* numPortsFound)

 		/* Attempt to open the serial port */
 		portFd = open(portName,
-					  #if __linux__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+					  #if __linux__ || __CYGWIN__ || __QNXNTO__
 					  O_RDWR | O_NOCTTY );
-					  #elif __APPLE__
+					  #elif __APPLE__ || __NUTTX__
 					  O_RDWR | O_NOCTTY | O_NONBLOCK);
 					  #else
 					  #error "Unknown System"
@@ -6,7 +6,7 @@ VnError VnCriticalSection_initialize(VnCriticalSection *cs)

 	InitializeCriticalSection(&cs->handle);

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	if (pthread_mutex_init(&cs->handle, NULL))
 		return E_UNKNOWN;
@@ -24,7 +24,7 @@ VnError VnCriticalSection_deinitialize(VnCriticalSection *cs)

 	DeleteCriticalSection(&cs->handle);

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	if (pthread_mutex_destroy(&cs->handle))
 		return E_UNKNOWN;
@@ -42,7 +42,7 @@ VnError VnCriticalSection_enter(VnCriticalSection *cs)

 	EnterCriticalSection(&cs->handle);

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	if (pthread_mutex_lock(&cs->handle))
 		return E_UNKNOWN;
@@ -60,7 +60,7 @@ VnError VnCriticalSection_leave(VnCriticalSection *cs)

 	LeaveCriticalSection(&cs->handle);

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	if (pthread_mutex_unlock(&cs->handle))
 		return E_UNKNOWN;
@@ -1,5 +1,5 @@
 /* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
-#ifdef __linux__
+#if defined(__linux__) || defined(__NUTTX__)
 	/* Works for Ubuntu 15.10 */
 	#define _POSIX_C_SOURCE 199309L
 #elif defined __CYGWIN__
@@ -100,7 +100,7 @@ VnError VnEvent_waitMs(VnEvent *e, uint32_t timeoutMs)

 	return E_UNKNOWN;

-	#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	return VnEvent_waitUs(e, timeoutMs * 1000);

@@ -218,7 +218,7 @@ VnError VnEvent_signal(VnEvent *e)
 	if (!SetEvent(e->handle))
 		return E_UNKNOWN;

-	#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	if (pthread_mutex_lock(&e->mutex))
 		return E_UNKNOWN;
@@ -2,7 +2,7 @@

 #if _WIN32
 	/* Nothing to do. */
-#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
 	#include <fcntl.h>
 	#include <errno.h>
 	#include <termios.h>
@@ -31,7 +31,7 @@ VnError VnSerialPort_initialize(VnSerialPort *sp)
 	#if _WIN32
 	sp->handle = NULL;
 	VnCriticalSection_initialize(&sp->readWriteCS);
-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
 	sp->handle = -1;
 	#else
 	#error "Unknown System"
@@ -196,9 +196,9 @@ VnError VnSerialPort_open_internal(VnSerialPort *serialport, char const *portNam

 	portFd = open(
 		portName,
-		#if __linux__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+		#if __linux__ || __CYGWIN__ || __QNXNTO__
 		O_RDWR | O_NOCTTY);
-		#elif __APPLE__
+		#elif __APPLE__ || __NUTTX__
 		O_RDWR | O_NOCTTY | O_NONBLOCK);
 		#else
 		#error "Unknown System"
@@ -268,9 +268,9 @@ VnError VnSerialPort_open_internal(VnSerialPort *serialport, char const *portNam
 	}

 	/* Set baudrate, 8n1, no modem control, enable receiving characters. */
-	#if __linux__ || __QNXNTO__ || __CYGWIN__ || defined __NUTTX__
+	#if __linux__ || __QNXNTO__ || __CYGWIN__
 	portSettings.c_cflag = baudrateFlag;
-	#elif defined(__APPLE__)
+	#elif defined(__APPLE__) || __NUTTX__
 	cfsetspeed(&portSettings, baudrateFlag);
 	#endif
 	portSettings.c_cflag |= CS8 | CLOCAL | CREAD;
@@ -315,7 +315,7 @@ VnError VnSerialPort_close_internal(VnSerialPort *serialport, bool checkAndToggl

 	serialport->handle = NULL;

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	if (close(serialport->handle) == -1)
 		return E_UNKNOWN;
@@ -338,7 +338,7 @@ VnError VnSerialPort_closeAfterUserUnpluggedSerialPort(VnSerialPort *serialport)

 	serialport->handle = NULL;

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	if (close(serialport->handle) == -1)
 		return E_UNKNOWN;
@@ -361,7 +361,7 @@ bool VnSerialPort_isOpen(VnSerialPort *serialport)
 {
 	#if defined(_WIN32)
 	return serialport->handle != NULL;
-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
 	return serialport->handle != -1;
 	#else
 	#error "Unknown System"
@@ -395,7 +395,7 @@ VnError VnSerialPort_read(VnSerialPort *serialport, char *buffer, size_t numOfBy
 	OVERLAPPED overlapped;
 	BOOL result;
 	DWORD numOfBytesTransferred;
-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
 	int result;
 	#else
 	#error "Unknown System"
@@ -436,7 +436,7 @@ VnError VnSerialPort_read(VnSerialPort *serialport, char *buffer, size_t numOfBy
 	if (!result)
 		return E_UNKNOWN;

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	result = read(
 		serialport->handle,
@@ -461,7 +461,7 @@ VnError VnSerialPort_write(VnSerialPort *sp, char const *data, size_t length)
 	DWORD numOfBytesWritten;
 	BOOL result;
 	OVERLAPPED overlapped;
-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
 	size_t numOfBytesWritten;
 	#else
 	#error "Unknown System"
@@ -508,7 +508,7 @@ VnError VnSerialPort_write(VnSerialPort *sp, char const *data, size_t length)
 	if (!result)
 		return E_UNKNOWN;

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	numOfBytesWritten = write(
 		sp->handle,
@@ -565,7 +565,7 @@ void VnSerialPort_handleSerialPortNotifications(void* routineData)
 		sp->handle,
 		EV_RXCHAR | EV_ERR | EV_RX80FULL);

-	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+	#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 	fd_set readfs;
 	int error;
@@ -672,7 +672,7 @@ void VnSerialPort_handleSerialPortNotifications(void* routineData)
 			continue;
 		}

-		#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
+		#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__

 		FD_ZERO(&readfs);
 		FD_SET(sp->handle, &readfs);
@@ -7,6 +7,7 @@
 	#include <unistd.h>
 	#include <stddef.h>
 	#include <pthread.h>
+	#include <sched.h>
 #endif

 #undef __cplusplus
@@ -77,10 +78,22 @@ VnError VnThread_startNew(VnThread *thread, VnThread_StartRoutine startRoutine,

 	#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)

+	pthread_attr_t attr;
+	pthread_attr_init(&attr);
+	pthread_attr_setstacksize(&attr, 5120);
+
+	// schedule policy FIFO
+	pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
+
+	// priority
+	struct sched_param param;
+	pthread_attr_getschedparam(&attr, &param);
+	param.sched_priority = sched_get_priority_max(SCHED_FIFO);
+	pthread_attr_setschedparam(&attr, &param);

 	errorCode = pthread_create(
 		&(thread->handle),
-		NULL,
+		&attr,
 		VnThread_intermediateStartRoutine,
 		starter);

@@ -1,5 +1,5 @@
 /* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
-#ifdef __linux__
+#if defined(__linux__) || defined(__NUTTX__)
 	/* Works for Ubuntu 15.10 */
 	#define _POSIX_C_SOURCE 199309L
 #elif defined __CYGWIN__
@@ -40,5 +40,5 @@ px4_add_module(
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
+		mixer_module
 	)
-
@@ -583,7 +583,7 @@ public:

 	bool isAllNan() const
 	{
-		const Matrix<float, M, N> &self = *this;
+		const Matrix<Type, M, N> &self = *this;
 		bool result = true;

 		for (size_t i = 0; i < M; i++) {
@@ -597,7 +597,7 @@ public:

 	bool isAllFinite() const
 	{
-		const Matrix<float, M, N> &self = *this;
+		const Matrix<Type, M, N> &self = *this;

 		for (size_t i = 0; i < M; i++) {
 			for (size_t j = 0; j < N; j++) {
@@ -71,6 +71,31 @@ public:
 		return (*this).cross(b);
 	}

+	/**
+	 * @brief rotate vector
+	 *
+	 * @param[in] angle [rad] The angle around the positive out of plane axis.
+	 */
+	void rotate(const Type angle)
+	{
+		Vector2 &v(*this);
+		Type sin_angle = sin(angle);
+		Type cos_angle = cos(angle);
+		v = Vector2(
+			    cos_angle * v(0) - sin_angle * v(1),
+			    sin_angle * v(0) + cos_angle * v(1)
+		    );
+	}
+
+	/**
+	 * @brief Transform vector to new coordinate system
+	 *
+	 * @param[in] angle [rad] The rotation of new coordinate system around the positive out of plane axis.
+	 */
+	void transform(const Type angle)
+	{
+		this->rotate(-angle);
+	}
 };


@@ -64,4 +64,20 @@ TEST(MatrixVector2Test, Vector2)
 	Vector2f h(g);
 	EXPECT_FLOAT_EQ(h(0), 1.23f);
 	EXPECT_FLOAT_EQ(h(1), 423.4f);
+
+	// rotate 2D vector
+	Vector2f i(1.0f, 0.0f);
+	i.rotate(M_PI_PRECISE / 4.f);
+	EXPECT_FLOAT_EQ(i(0), i(1));
+	i.rotate(M_PI_PRECISE / 4.f);
+	EXPECT_FLOAT_EQ(i(0), 0.f);
+	EXPECT_FLOAT_EQ(i(1), 1.f);
+
+	// transform vector
+	Vector2f j(1.0f, 0.0f);
+	j.transform(-M_PI_PRECISE / 4.f);
+	EXPECT_FLOAT_EQ(j(0), j(1));
+	j.transform(-M_PI_PRECISE / 4.f);
+	EXPECT_FLOAT_EQ(j(0), 0.f);
+	EXPECT_FLOAT_EQ(j(1), 1.f);
 }
@@ -288,7 +288,7 @@ TECSControl::STERateLimit TECSControl::_calculateTotalEnergyRateLimit(const Para
 	TECSControl::STERateLimit limit;
 	// Calculate the specific total energy rate limits from the max throttle limits
 	limit.STE_rate_max = math::max(param.max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
-	limit.STE_rate_min = - math::max(param.max_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
+	limit.STE_rate_min = - math::max(param.min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;

 	return limit;
 }
@@ -619,12 +619,8 @@ float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, con
 	float new_setpoint{tas_setpoint};
 	const float percent_undersped = _control.getRatioUndersped();

-	// Set the TAS demand to the minimum value if an underspeed or
-	// or a uncontrolled descent condition exists to maximise climb rate
-	if (_uncommanded_descent_recovery) {
-		new_setpoint = tas_min;
-
-	} else if (percent_undersped > FLT_EPSILON) {
+	// Set the TAS demand to the minimum value if an underspeed condition exists to maximise climb rate
+	if (percent_undersped > FLT_EPSILON) {
 		// TAS setpoint is reset from external setpoint every time tecs is called, so the interpolation is still
 		// between current setpoint and mininimum airspeed here (it's not feeding the newly adjusted setpoint
 		// from this line back into this method each time).
@@ -638,47 +634,6 @@ float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, con
 	return new_setpoint;
 }

-void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas,
-				       float tas_setpoint)
-{
-	/*
-	 * This function detects a condition that can occur when the demanded airspeed is greater than the
-	 * aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce altitude
-	 * while attempting to maintain speed.
-	*/
-
-	// Calculate specific energy demands in units of (m**2/sec**2)
-	const float SPE_setpoint = altitude_setpoint * CONSTANTS_ONE_G; // potential energy
-	const float SKE_setpoint = 0.5f * altitude_setpoint * altitude_setpoint; // kinetic energy
-
-	// Calculate specific energies in units of (m**2/sec**2)
-	const float SPE_estimate = altitude * CONSTANTS_ONE_G; // potential energy
-	const float SKE_estimate = 0.5f * tas * tas; // kinetic energy
-
-	// Calculate total energy error
-	const float SPE_error = SPE_setpoint - SPE_estimate;
-	const float SKE_error = SKE_setpoint - SKE_estimate;
-	const float STE_error = SPE_error + SKE_error;
-
-	const bool underspeed_detected = _control.getRatioUndersped() > FLT_EPSILON;
-
-	// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
-	const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (STE_error > 200.0f)
-				&& (_control.getSteRate() < 0.0f)
-				&& (_control.getThrottleSetpoint() >= throttle_setpoint_max * 0.9f);
-
-	// If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
-	const bool exit_mode = _uncommanded_descent_recovery && (underspeed_detected || (STE_error < 0.0f));
-
-	if (enter_mode) {
-		_uncommanded_descent_recovery = true;
-
-	} else if (exit_mode) {
-		_uncommanded_descent_recovery = false;
-
-	}
-}
-
 void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
 		      const float eas_to_tas)
 {
@@ -774,10 +729,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set

 		_control.update(dt, control_setpoint, control_input, _control_param, _control_flag);

-		// Detect an uncommanded descent caused by an unachievable airspeed demand
-		_detect_uncommanded_descent(throttle_setpoint_max, altitude, hgt_setpoint, equivalent_airspeed * eas_to_tas,
-					    control_setpoint.tas_setpoint);
-
 		// Update time stamps
 		_update_timestamp = now;

@@ -786,9 +737,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		if (_control.getRatioUndersped() > FLT_EPSILON) {
 			_tecs_mode = ECL_TECS_MODE_UNDERSPEED;

-		} else if (_uncommanded_descent_recovery) {
-			_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
-
 		} else {
 			// This is the default operation mode
 			_tecs_mode = ECL_TECS_MODE_NORMAL;
@@ -135,7 +135,7 @@ public:
 		float jerk_max;		///< Magnitude of the maximum jerk allowed [m/s³].
 		float vert_accel_limit;	///< Magnitude of the maximum vertical acceleration allowed [m/s²].
 		float max_climb_rate;	///< Climb rate produced by max allowed throttle [m/s].
-		float max_sink_rate;	///< Maximum safe sink rate [m/s].
+		float max_sink_rate;	///< Maximum sink rate (with min throttle, max speed) [m/s].
 	};

 public:
@@ -189,7 +189,8 @@ public:
 	 */
 	struct Param {
 		// Vehicle specific params
-		float max_sink_rate;			///< Maximum safe sink rate [m/s].
+		float max_sink_rate;			///< Maximum sink rate (with min throttle and max speed) [m/s].
+		float min_sink_rate;			///< Minimum sink rate (with min throttle and trim speed) [m/s].
 		float max_climb_rate;			///< Climb rate produced by max allowed throttle [m/s].
 		float vert_accel_limit;			///< Magnitude of the maximum vertical acceleration allowed [m/s²].
 		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
@@ -542,9 +543,7 @@ class TECS
 public:
 	enum ECL_TECS_MODE {
 		ECL_TECS_MODE_NORMAL = 0,
-		ECL_TECS_MODE_UNDERSPEED,
-		ECL_TECS_MODE_BAD_DESCENT,
-		ECL_TECS_MODE_CLIMBOUT
+		ECL_TECS_MODE_UNDERSPEED
 	};

 	struct DebugOutput {
@@ -610,7 +609,8 @@ public:
 	void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;};
 	void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; };

-	void set_max_sink_rate(float sink_rate) { _control_param.max_sink_rate = sink_rate; _reference_param.max_sink_rate = sink_rate; };
+	void set_max_sink_rate(float max_sink_rate) { _control_param.max_sink_rate = max_sink_rate; _reference_param.max_sink_rate = max_sink_rate; };
+	void set_min_sink_rate(float min_sink_rate) { _control_param.min_sink_rate = min_sink_rate; };
 	void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; };

 	void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; };
@@ -669,9 +669,6 @@ private:
 	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
 	float _equivalent_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)

-	// controller mode logic
-	bool _uncommanded_descent_recovery{false};			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
-
 	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
 	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)

@@ -696,8 +693,9 @@ private:
 	};
 	/// Control parameters.
 	TECSControl::Param _control_param{
-		.max_sink_rate = 2.0f,
-		.max_climb_rate = 2.0f,
+		.max_sink_rate = 5.0f,
+		.min_sink_rate = 2.0f,
+		.max_climb_rate = 5.0f,
 		.vert_accel_limit = 0.0f,
 		.equivalent_airspeed_trim = 15.0f,
 		.tas_min = 3.0f,
@@ -731,11 +729,5 @@ private:
 	 * Update the desired airspeed
 	 */
 	float _update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas);
-
-	/**
-	 * Detect an uncommanded descent
-	 */
-	void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas,
-					 float tas_setpoint);
 };

@@ -127,7 +127,6 @@ private:
 	bool _data_stuck_check_enabled{false};
 	bool _innovation_check_enabled{false};
 	bool _load_factor_check_enabled{false};
-	bool _data_variation_check_enabled{false};

 	// airspeed scale validity check
 	static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -45,7 +45,6 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu
 	_failsafe_flags.local_position_invalid_relaxed = true;
 	_failsafe_flags.local_velocity_invalid = true;
 	_failsafe_flags.global_position_invalid = true;
-	_failsafe_flags.gps_position_invalid = true;
 	_failsafe_flags.auto_mission_missing = true;
 	_failsafe_flags.offboard_control_signal_lost = true;
 	_failsafe_flags.home_position_invalid = true;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -37,9 +37,6 @@ using namespace time_literals;

 EstimatorChecks::EstimatorChecks()
 {
-	_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
-	_vehicle_gps_position_valid.set_hysteresis_time_from(true, 0);
-
 	// initially set to failed
 	_last_lpos_fail_time_us = hrt_absolute_time();
 	_last_lpos_relaxed_fail_time_us = _last_lpos_fail_time_us;
@@ -119,13 +116,9 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 	}

 	// set mode requirements
-	const bool condition_gps_position_was_valid = !reporter.failsafeFlags().gps_position_invalid;
 	setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position,
 				reporter.failsafeFlags());

-	if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) {
-		gpsNoLongerValid(context, reporter);
-	}

 	lowPositionAccuracy(context, reporter, lpos);
 }
@@ -280,7 +273,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 	}

 	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
-	if (!context.isArmed() && _param_sys_has_gps.get()) {
+	if (_param_sys_has_gps.get()) {
 		const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
 		const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;

@@ -288,7 +281,30 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 			reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
 		}

-		if (ekf_gps_check_fail) {
+		if (context.isArmed()) {
+
+			if (_gps_was_fused && !ekf_gps_fusion) {
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_warning(reporter.mavlink_log_pub(), "GNSS data fusion stopped\t");
+				}
+
+				events::send(events::ID("check_estimator_gnss_fusion_stopped"), {events::Log::Error, events::LogInternal::Info},
+					     "GNSS data fusion stopped");
+
+			} else if (!_gps_was_fused && ekf_gps_fusion) {
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_info(reporter.mavlink_log_pub(), "GNSS data fusion started\t");
+				}
+
+				events::send(events::ID("check_estimator_gnss_fusion_started"), {events::Log::Info, events::LogInternal::Info},
+					     "GNSS data fusion started");
+			}
+		}
+
+		_gps_was_fused = ekf_gps_fusion;
+
+		if (!context.isArmed() && ekf_gps_check_fail) {
 			NavModes required_groups_gps = required_groups;
 			events::Log log_level = events::Log::Error;

@@ -656,21 +672,6 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s
 	}
 }

-void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter) const
-{
-	PX4_DEBUG("GPS no longer valid");
-
-	// report GPS failure if armed and the global position estimate is still valid
-	if (context.isArmed() && !reporter.failsafeFlags().global_position_invalid) {
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_warning(reporter.mavlink_log_pub(), "GPS no longer valid\t");
-		}
-
-		events::send(events::ID("check_estimator_gps_lost"), {events::Log::Error, events::LogInternal::Info},
-			     "GPS no longer valid");
-	}
-}
-
 void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter,
 		const vehicle_local_position_s &lpos) const
 {
@@ -798,24 +799,6 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 	}

 	failsafe_flags.angular_velocity_invalid = angular_velocity_invalid;
-
-
-	// gps
-	if (vehicle_gps_position.timestamp != 0) {
-		bool time = (now < vehicle_gps_position.timestamp + 1_s);
-
-		bool fix = vehicle_gps_position.fix_type >= 2;
-		bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get();
-		bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get();
-		bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
-		bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL);
-
-		_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, now);
-		failsafe_flags.gps_position_invalid = !_vehicle_gps_position_valid.get_state();
-
-	} else {
-		failsafe_flags.gps_position_invalid = true;
-	}
 }

 bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy,
@@ -66,7 +66,6 @@ private:
 				       const vehicle_local_position_s &lpos);

 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
-	void gpsNoLongerValid(const Context &context, Report &reporter) const;
 	void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
 	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
 				     const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
@@ -100,11 +99,10 @@ private:
 	bool		_nav_test_passed{false};	///< true if the post takeoff navigation test has passed
 	bool		_nav_test_failed{false};	///< true if the post takeoff navigation test has failed

-	static constexpr hrt_abstime GPS_VALID_TIME{3_s};
-	systemlib::Hysteresis _vehicle_gps_position_valid{false};
-
 	bool _position_reliant_on_optical_flow{false};

+	bool _gps_was_fused{false};
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
 					(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
@@ -116,7 +114,6 @@ private:
 					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
 					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
-					(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
 					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
 					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph
@@ -65,6 +65,7 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter

 			if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) {
 				is_calibration_valid = true;
+				num_enabled_and_valid_calibration++;

 			} else {
 				int calibration_index = calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -37,6 +37,8 @@
 #include <lib/geo/geo.h>
 #include "commander_helper.h"

+using namespace time_literals;
+
 HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags)
 	: _failsafe_flags(failsafe_flags)
 {
@@ -69,20 +71,15 @@ bool HomePosition::hasMovedFromCurrentHomeLocation()
 			eph = gpos.eph;
 			epv = gpos.epv;

-		} else if (!_failsafe_flags.gps_position_invalid) {
-			sensor_gps_s gps;
-			_vehicle_gps_position_sub.copy(&gps);
-			const double lat = static_cast<double>(gps.lat) * 1e-7;
-			const double lon = static_cast<double>(gps.lon) * 1e-7;
-			const float alt = static_cast<float>(gps.alt) * 1e-3f;
+		} else if (_gps_position_for_home_valid) {

 			get_distance_to_point_global_wgs84(_home_position_pub.get().lat, _home_position_pub.get().lon,
 							   _home_position_pub.get().alt,
-							   lat, lon, alt,
+							   _gps_lat, _gps_lon, _gps_alt,
 							   &home_dist_xy, &home_dist_z);

-			eph = gps.eph;
-			epv = gps.epv;
+			eph = _gps_eph;
+			epv = _gps_epv;
 		}
 	}

@@ -114,14 +111,9 @@ bool HomePosition::setHomePosition(bool force)
 		setHomePosValid();
 		updated = true;

-	} else if (!_failsafe_flags.gps_position_invalid) {
+	} else if (_gps_position_for_home_valid) {
 		// Set home using GNSS position
-		sensor_gps_s gps_pos;
-		_vehicle_gps_position_sub.copy(&gps_pos);
-		const double lat = static_cast<double>(gps_pos.lat) * 1e-7;
-		const double lon = static_cast<double>(gps_pos.lon) * 1e-7;
-		const float alt = static_cast<float>(gps_pos.alt) * 1e-3f;
-		fillGlobalHomePos(home, lat, lon, alt);
+		fillGlobalHomePos(home, _gps_lat, _gps_lon, _gps_alt);
 		setHomePosValid();
 		updated = true;

@@ -203,24 +195,18 @@ void HomePosition::setInAirHomePosition()
 			home.timestamp = hrt_absolute_time();
 			_home_position_pub.update();

-		} else if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.gps_position_invalid) {
+		} else if (!_failsafe_flags.local_position_invalid && _gps_position_for_home_valid) {
 			// Back-compute lon, lat and alt of home position given the local home position
 			// and current positions in local and global (GNSS raw) frames
 			const vehicle_local_position_s &lpos = _local_position_sub.get();
-			sensor_gps_s gps;
-			_vehicle_gps_position_sub.copy(&gps);

-			const double lat = static_cast<double>(gps.lat) * 1e-7;
-			const double lon = static_cast<double>(gps.lon) * 1e-7;
-			const float alt = static_cast<float>(gps.alt) * 1e-3f;
-
-			MapProjection ref_pos{lat, lon};
+			MapProjection ref_pos{_gps_lat, _gps_lon};

 			double home_lat;
 			double home_lon;
 			ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon);

-			const float home_alt = alt + home.z;
+			const float home_alt = _gps_alt + home.z;
 			fillGlobalHomePos(home, home_lat, home_lon, home_alt);

 			setHomePosValid();
@@ -314,6 +300,25 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 	_local_position_sub.update();
 	_global_position_sub.update();

+	if (_vehicle_gps_position_sub.updated()) {
+		sensor_gps_s vehicle_gps_position;
+		_vehicle_gps_position_sub.copy(&vehicle_gps_position);
+
+		_gps_lat = static_cast<double>(vehicle_gps_position.lat) * 1e-7;
+		_gps_lon = static_cast<double>(vehicle_gps_position.lon) * 1e-7;
+		_gps_alt = static_cast<float>(vehicle_gps_position.alt) * 1e-3f;
+		_gps_eph = vehicle_gps_position.eph;
+		_gps_epv = vehicle_gps_position.epv;
+
+		const hrt_abstime now = hrt_absolute_time();
+		const bool time = (now < vehicle_gps_position.timestamp + 1_s);
+		const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
+		const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
+		const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
+		const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
+		_gps_position_for_home_valid = time && fix && eph && epv && evh;
+	}
+
 	const vehicle_local_position_s &lpos = _local_position_sub.get();
 	const auto &home = _home_position_pub.get();

@@ -328,7 +333,7 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 	if (check_if_changed && set_automatically) {
 		const bool can_set_home_lpos_first_time = !home.valid_lpos && !_failsafe_flags.local_position_invalid;
 		const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt)
-				&& (!_failsafe_flags.global_position_invalid || !_failsafe_flags.gps_position_invalid));
+				&& (!_failsafe_flags.global_position_invalid || _gps_position_for_home_valid));
 		const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global);

 		if (can_set_home_lpos_first_time
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -41,6 +41,11 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/failsafe_flags.h>

+static constexpr int kHomePositionGPSRequiredFixType = 2;
+static constexpr float kHomePositionGPSRequiredEPH = 5.f;
+static constexpr float kHomePositionGPSRequiredEPV = 10.f;
+static constexpr float kHomePositionGPSRequiredEVH = 1.f;
+
 class HomePosition
 {
 public:
@@ -75,4 +80,10 @@ private:
 	uint8_t							_heading_reset_counter{0};
 	bool							_valid{false};
 	const failsafe_flags_s					&_failsafe_flags;
+	bool							_gps_position_for_home_valid{false};
+	double							_gps_lat{0};
+	double							_gps_lon{0};
+	float							_gps_alt{0.f};
+	float							_gps_eph{0.f};
+	float							_gps_epv{0.f};
 };
@@ -726,21 +726,6 @@ PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1);
 */
 PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5.f);

-/**
- * Vertical position error threshold.
- *
- * This is the vertical position error (EPV) threshold that will trigger a failsafe.
- * The default is appropriate for a multicopter. Can be increased for a fixed-wing.
- * If the previous position error was below this threshold, there is an additional
- * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
- *
- * @unit m
- * @min 0
- * @decimal 1
- * @group Commander
- */
-PARAM_DEFINE_FLOAT(COM_POS_FS_EPV, 10.f);
-
 /**
 * Horizontal velocity error threshold.
 *
@@ -82,6 +82,7 @@ px4_add_module(
 		3600
 	SRCS
 		EKF/airspeed_fusion.cpp
+		EKF/auxvel_fusion.cpp
 		EKF/baro_height_control.cpp
 		EKF/bias_estimator.cpp
 		EKF/control.cpp
@@ -33,6 +33,7 @@

 add_library(ecl_EKF
 	airspeed_fusion.cpp
+	auxvel_fusion.cpp
 	baro_height_control.cpp
 	bias_estimator.cpp
 	control.cpp
@@ -44,12 +44,8 @@ EKFGSF_yaw::EKFGSF_yaw()

 void EKFGSF_yaw::update(const imuSample &imu_sample,
 			bool run_EKF,			// set to true when flying or movement is suitable for yaw estimation
-			float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
 			const Vector3f &imu_gyro_bias)  // estimated rate gyro bias (rad/sec)
 {
-	// copy to class variables
-	_true_airspeed = airspeed;
-
 	// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
 	const float filter_coef = fminf(10.f * imu_sample.delta_vel_dt * _tilt_gain, 1.f);
 	const Vector3f accel = imu_sample.delta_vel / fmaxf(imu_sample.delta_vel_dt, 0.001f);
@@ -177,7 +173,7 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an

 		Vector3f accel = _ahrs_accel;

-		if (_true_airspeed > FLT_EPSILON) {
+		if (PX4_ISFINITE(_true_airspeed) && (_true_airspeed > FLT_EPSILON)) {
 			// Calculate body frame centripetal acceleration with assumption X axis is aligned with the airspeed vector
 			// Use cross product of body rate and body frame airspeed vector
 			const Vector3f centripetal_accel_bf = Vector3f(0.0f, _true_airspeed * ang_rate(2), - _true_airspeed * ang_rate(1));
@@ -428,10 +424,9 @@ float EKFGSF_yaw::ahrsCalcAccelGain() const
 	// see https://www.desmos.com/calculator/dbqbxvnwfg

 	float attenuation = 2.f;
-	const bool centripetal_accel_compensation_enabled = (_true_airspeed > FLT_EPSILON);
+	const bool centripetal_accel_compensation_enabled = PX4_ISFINITE(_true_airspeed) && (_true_airspeed > FLT_EPSILON);

-	if (centripetal_accel_compensation_enabled
-	    && _ahrs_accel_norm > CONSTANTS_ONE_G) {
+	if (centripetal_accel_compensation_enabled && (_ahrs_accel_norm > CONSTANTS_ONE_G)) {
 		attenuation = 1.f;
 	}

@@ -62,12 +62,14 @@ public:
 	// Update Filter States - this should be called whenever new IMU data is available
 	void update(const imuSample &imu_sample,
 		    bool run_EKF,  			// set to true when flying or movement is suitable for yaw estimation
-		    float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
 		    const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)

 	void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
 			 float accuracy);	   // 1-sigma accuracy of velocity measurement (m/s)

+
+	void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; }
+
 	// get solution data for logging
 	bool getLogData(float *yaw_composite,
 			float *yaw_composite_variance,
@@ -77,6 +79,7 @@ public:
 			float weight[N_MODELS_EKFGSF]) const;

 	bool isActive() const { return _ekf_gsf_vel_fuse_started; }
+
 	float getYaw() const { return _gsf_yaw; }
 	float getYawVar() const { return _gsf_yaw_variance; }

@@ -89,7 +92,7 @@ private:
 	const float _gyro_bias_gain{0.04f};	// gain applied to integral of gyro correction for complementary filter (1/sec)

 	// Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters
-	float _true_airspeed{};	// true airspeed used for centripetal accel compensation (m/s)
+	float _true_airspeed{NAN};	// true airspeed used for centripetal accel compensation (m/s)

 	struct {
 		Dcmf R{matrix::eye<float, 3>()}; // matrix that rotates a vector from body to earth frame
@@ -49,10 +49,91 @@

 #include <mathlib/mathlib.h>

-void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const
+void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
+{
+	// control activation and initialisation/reset of wind states required for airspeed fusion
+
+	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
+	const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
+	const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6);
+
+	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) {
+		_control_status.flags.wind = false;
+	}
+
+	// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
+	if (_control_status.flags.fixed_wing) {
+		if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
+			if (!_control_status.flags.fuse_aspd) {
+				_yawEstimator.setTrueAirspeed(_params.EKFGSF_tas_default);
+			}
+
+		} else {
+			_yawEstimator.setTrueAirspeed(0.f);
+		}
+	}
+
+	if (_params.arsp_thr <= 0.f) {
+		stopAirspeedFusion();
+		return;
+	}
+
+	if (_airspeed_buffer && _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed)) {
+
+		const airspeedSample &airspeed_sample = _airspeed_sample_delayed;
+
+		updateAirspeed(airspeed_sample, _aid_src_airspeed);
+
+		_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
+
+		const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos;
+		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
+
+		if (_control_status.flags.fuse_aspd) {
+			if (continuing_conditions_passing) {
+				if (is_airspeed_significant) {
+					fuseAirspeed(airspeed_sample, _aid_src_airspeed);
+				}
+
+				_yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed);
+
+				const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
+
+				if (is_fusion_failing) {
+					stopAirspeedFusion();
+				}
+
+			} else {
+				stopAirspeedFusion();
+			}
+
+		} 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
+			if (!_control_status.flags.wind) {
+				// activate the wind states
+				_control_status.flags.wind = true;
+				// reset the wind speed states and corresponding covariances
+				resetWindUsingAirspeed(airspeed_sample);
+			}
+
+			_control_status.flags.fuse_aspd = true;
+		}
+
+	} else if (_control_status.flags.fuse_aspd && !isRecent(_airspeed_sample_delayed.time_us, (uint64_t)1e6)) {
+		ECL_WARN("Airspeed data stopped");
+		stopAirspeedFusion();
+	}
+}
+
+void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const
 {
 	// reset flags
-	resetEstimatorAidStatus(airspeed);
+	resetEstimatorAidStatus(aid_src);

 	// Variance for true airspeed measurement - (m/sec)^2
 	const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
@@ -62,37 +143,37 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
 	float innov_var = 0.f;
 	sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);

-	airspeed.observation = airspeed_sample.true_airspeed;
-	airspeed.observation_variance = R;
-	airspeed.innovation = innov;
-	airspeed.innovation_variance = innov_var;
+	aid_src.observation = airspeed_sample.true_airspeed;
+	aid_src.observation_variance = R;
+	aid_src.innovation = innov;
+	aid_src.innovation_variance = innov_var;

-	airspeed.fusion_enabled = _control_status.flags.fuse_aspd;
+	aid_src.fusion_enabled = _control_status.flags.fuse_aspd;

-	airspeed.timestamp_sample = airspeed_sample.time_us;
+	aid_src.timestamp_sample = airspeed_sample.time_us;

 	const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f);
-	setEstimatorAidStatusTestRatio(airspeed, innov_gate);
+	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
 }

-void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed)
+void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)
 {
-	if (airspeed.innovation_rejected) {
+	if (aid_src.innovation_rejected) {
 		return;
 	}

 	// determine if we need the airspeed fusion to correct states other than wind
 	const bool update_wind_only = !_control_status.flags.wind_dead_reckoning;

-	const float innov_var = airspeed.innovation_variance;
+	const float innov_var = aid_src.innovation_variance;

-	if (innov_var < airspeed.observation_variance || innov_var < FLT_EPSILON) {
+	if (innov_var < aid_src.observation_variance || innov_var < FLT_EPSILON) {
 		// Reset the estimator covariance matrix
 		// if we are getting aiding from other sources, warn and reset the wind states and covariances only
 		const char *action_string = nullptr;

 		if (update_wind_only) {
-			resetWindUsingAirspeed();
+			resetWindUsingAirspeed(airspeed_sample);
 			action_string = "wind";

 		} else {
@@ -121,13 +202,23 @@ void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed)
 		}
 	}

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

-	airspeed.fused = is_fused;
+	aid_src.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;

 	if (is_fused) {
-		airspeed.time_last_fuse = _time_delayed_us;
+		aid_src.time_last_fuse = _time_delayed_us;
+	}
+}
+
+void Ekf::stopAirspeedFusion()
+{
+	if (_control_status.flags.fuse_aspd) {
+		ECL_INFO("stopping airspeed fusion");
+		resetEstimatorAidStatus(_aid_src_airspeed);
+		_yawEstimator.setTrueAirspeed(NAN);
+		_control_status.flags.fuse_aspd = false;
 	}
 }

@@ -138,32 +229,60 @@ float Ekf::getTrueAirspeed() const

 void Ekf::resetWind()
 {
-	if (_control_status.flags.fuse_aspd) {
-		resetWindUsingAirspeed();
+	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
+		resetWindUsingAirspeed(_airspeed_sample_delayed);

 	} else {
 		resetWindToZero();
 	}
 }

-/*
- * Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
-*/
-void Ekf::resetWindUsingAirspeed()
+void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
 {
 	const float euler_yaw = getEulerYaw(_R_to_earth);

 	// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
-	_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
-	_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
+	_state.wind_vel(0) = _state.vel(0) - airspeed_sample.true_airspeed * cosf(euler_yaw);
+	_state.wind_vel(1) = _state.vel(1) - airspeed_sample.true_airspeed * sinf(euler_yaw);

 	ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1));

-	resetWindCovarianceUsingAirspeed();
+	resetWindCovarianceUsingAirspeed(airspeed_sample);

 	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
 }

+void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample)
+{
+	// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
+	// TODO: explicitly include the sideslip angle in the derivation
+	const float euler_yaw = getEulerYaw(_R_to_earth);
+	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
+	constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
+	const float initial_wind_var_body_y = sq(airspeed_sample.true_airspeed * sinf(initial_sideslip_uncertainty));
+	constexpr float R_yaw = sq(math::radians(10.0f));
+
+	const float cos_yaw = cosf(euler_yaw);
+	const float sin_yaw = sinf(euler_yaw);
+
+	// rotate wind velocity into earth frame aligned with vehicle yaw
+	const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw;
+	const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;
+
+	// it is safer to remove all existing correlations to other states at this time
+	P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
+
+	P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
+	P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
+		    initial_wind_var_body_y * sin_yaw * cos_yaw;
+	P(23, 22) = P(22, 23);
+	P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
+
+	// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
+	P(22, 22) += P(4, 4);
+	P(23, 23) += P(5, 5);
+}
+
 void Ekf::resetWindToZero()
 {
 	ECL_INFO("reset wind to zero");
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   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 "ekf.h"
+
+void Ekf::controlAuxVelFusion()
+{
+	if (_auxvel_buffer) {
+		auxVelSample auxvel_sample_delayed;
+
+		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) {
+
+			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);
+
+			if (isHorizontalAidingActive()) {
+				_aid_src_aux_vel.fusion_enabled = true;
+				fuseVelocity(_aid_src_aux_vel);
+			}
+		}
+	}
+}
+
+void Ekf::stopAuxVelFusion()
+{
+	ECL_INFO("stopping aux vel fusion");
+	//_control_status.flags.aux_vel = false;
+	resetEstimatorAidStatus(_aid_src_aux_vel);
+}
@@ -70,7 +70,8 @@ static constexpr uint64_t BARO_MAX_INTERVAL     = 200e3;  ///< Maximum allowable
 static constexpr uint64_t EV_MAX_INTERVAL       = 200e3;  ///< Maximum allowable time interval between external vision system measurements (uSec)
 static constexpr uint64_t GNSS_MAX_INTERVAL     = 500e3;  ///< Maximum allowable time interval between GNSS measurements (uSec)
 static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
-static constexpr uint64_t RNG_MAX_INTERVAL      = 200e3;  ///< Maximum allowable time interval between range finder  measurements (uSec)
+static constexpr uint64_t RNG_MAX_INTERVAL      = 200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
+static constexpr uint64_t MAG_MAX_INTERVAL      = 500e3;  ///< Maximum allowable time interval between magnetic field measurements (uSec)

 // bad accelerometer detection and mitigation
 static constexpr uint64_t BADACC_PROBATION = 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
@@ -102,99 +102,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 		}
 	}

-	if (_gps_buffer) {
-		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
-
-		// check for arrival of new sensor data at the fusion time horizon
-		_time_prev_gps_us = _gps_sample_delayed.time_us;
-		_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
-
-		if (_gps_data_ready) {
-			// correct velocity for offset relative to IMU
-			const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
-			const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-			_gps_sample_delayed.vel -= vel_offset_earth;
-
-			// correct position and height for offset relative to IMU
-			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_gps_sample_delayed.pos -= pos_offset_earth.xy();
-			_gps_sample_delayed.hgt += pos_offset_earth(2);
-
-			// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
-			if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc)
-			    && _gps_sample_delayed.vel.isAllFinite()
-			   ) {
-				_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise));
-			}
-		}
-	}
-
-	if (_range_buffer) {
-		// Get range data from buffer and check validity
-		_rng_data_ready = _range_buffer->pop_first_older_than(imu_delayed.time_us, _range_sensor.getSampleAddress());
-		_range_sensor.setDataReadiness(_rng_data_ready);
-
-		// update range sensor angle parameters in case they have changed
-		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
-		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
-
-		_range_sensor.runChecks(imu_delayed.time_us, _R_to_earth);
-
-		if (_range_sensor.isDataHealthy()) {
-			// correct the range data for position offset relative to the IMU
-			const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
-			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
-
-			// Run the kinematic consistency check when not moving horizontally
-			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
-			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
-
-				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
-				const float var = sq(_params.range_noise) + dist_dependant_var;
-
-				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), imu_delayed.time_us);
-			}
-
-		} else {
-			// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
-			// and are on the ground, then synthesise a measurement at the expected on ground value
-			if (!_control_status.flags.in_air
-			&& _range_sensor.isRegularlySendingData()
-			&& _range_sensor.isDataReady()) {
-
-				_range_sensor.setRange(_params.rng_gnd_clearance);
-				_range_sensor.setValidity(true); // bypass the checks
-			}
-		}
-
-		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
-	}
-
-	if (_flow_buffer) {
-		// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
-		// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
-		// in this case we need to empty the buffer
-		if (!_flow_data_ready || (!_control_status.flags.opt_flow && !_hagl_sensor_status.flags.flow)) {
-			_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
-		}
-	}
-
-	if (_airspeed_buffer) {
-		_tas_data_ready = _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed);
-	}
-
-	// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
-	runYawEKFGSF(imu_delayed);
-
 	// control use of observations for aiding
 	controlMagFusion();
 	controlOpticalFlowFusion(imu_delayed);
-	controlGpsFusion();
-	controlAirDataFusion();
+	controlGpsFusion(imu_delayed);
+	controlAirDataFusion(imu_delayed);
 	controlBetaFusion(imu_delayed);
 	controlDragFusion();
 	controlHeightFusion(imu_delayed);
@@ -217,215 +129,3 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	// check if we are no longer fusing measurements that directly constrain velocity drift
 	updateDeadReckoningStatus();
 }
-
-void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
-{
-	if (!(_params.gnss_ctrl & GnssCtrl::YAW)
-	    || _control_status.flags.gps_yaw_fault) {
-
-		stopGpsYawFusion();
-		return;
-	}
-
-	updateGpsYaw(gps_sample);
-
-	const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
-
-	if (is_new_data_available) {
-
-		const bool continuing_conditions_passing = !gps_checks_failing;
-
-		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
-
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& _control_status.flags.tilt_align
-				&& gps_checks_passing
-				&& !is_gps_yaw_data_intermittent
-				&& !_gps_intermittent;
-
-		if (_control_status.flags.gps_yaw) {
-
-			if (continuing_conditions_passing) {
-
-				fuseGpsYaw();
-
-				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
-
-				if (is_fusion_failing) {
-					if (_nb_gps_yaw_reset_available > 0) {
-						// Data seems good, attempt a reset
-						resetYawToGps(gps_sample.yaw);
-
-						if (_control_status.flags.in_air) {
-							_nb_gps_yaw_reset_available--;
-						}
-
-					} else if (starting_conditions_passing) {
-						// Data seems good, but previous reset did not fix the issue
-						// something else must be wrong, declare the sensor faulty and stop the fusion
-						_control_status.flags.gps_yaw_fault = true;
-						stopGpsYawFusion();
-
-					} else {
-						// A reset did not fix the issue but all the starting checks are not passing
-						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-						stopGpsYawFusion();
-					}
-
-					// TODO: should we give a new reset credit when the fusion does not fail for some time?
-				}
-
-			} else {
-				// Stop GPS yaw fusion but do not declare it faulty
-				stopGpsYawFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-				// Try to activate GPS yaw fusion
-				startGpsYawFusion(gps_sample);
-
-				if (_control_status.flags.gps_yaw) {
-					_nb_gps_yaw_reset_available = 1;
-				}
-			}
-		}
-
-	} else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
-		// No yaw data in the message anymore. Stop until it comes back.
-		stopGpsYawFusion();
-	}
-
-	// Before takeoff, we do not want to continue to rely on the current heading
-	// if we had to stop the fusion
-	if (!_control_status.flags.in_air
-	    && !_control_status.flags.gps_yaw
-	    && _control_status_prev.flags.gps_yaw) {
-		_control_status.flags.yaw_align = false;
-	}
-}
-
-void Ekf::controlAirDataFusion()
-{
-	// control activation and initialisation/reset of wind states required for airspeed fusion
-
-	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
-	const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
-	const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6);
-
-	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) {
-		_control_status.flags.wind = false;
-	}
-
-	if (_params.arsp_thr <= 0.f) {
-		stopAirspeedFusion();
-		return;
-	}
-
-	if (_tas_data_ready) {
-		updateAirspeed(_airspeed_sample_delayed, _aid_src_airspeed);
-
-		_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
-
-		const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos;
-		const bool is_airspeed_significant = _airspeed_sample_delayed.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
-
-		if (_control_status.flags.fuse_aspd) {
-			if (continuing_conditions_passing) {
-				if (is_airspeed_significant) {
-					fuseAirspeed(_aid_src_airspeed);
-				}
-
-				const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
-
-				if (is_fusion_failing) {
-					stopAirspeedFusion();
-				}
-
-			} else {
-				stopAirspeedFusion();
-			}
-
-		} else if (starting_conditions_passing) {
-			startAirspeedFusion();
-		}
-
-	} else if (_control_status.flags.fuse_aspd && !isRecent(_airspeed_sample_delayed.time_us, (uint64_t)1e6)) {
-		ECL_WARN("Airspeed data stopped");
-		stopAirspeedFusion();
-	}
-}
-
-void Ekf::controlBetaFusion(const imuSample &imu_delayed)
-{
-	_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
-		&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
-
-	if (_control_status.flags.fuse_beta) {
-
-		// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally:
-		const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
-
-		if (beta_fusion_time_triggered) {
-
-			updateSideslip(_aid_src_sideslip);
-			_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
-
-			// If starting wind state estimation, reset the wind states and covariances before fusing any data
-			if (!_control_status.flags.wind) {
-				// activate the wind states
-				_control_status.flags.wind = true;
-				// reset the timeout timers to prevent repeated resets
-				_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
-				resetWind();
-			}
-
-			if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
-				fuseSideslip(_aid_src_sideslip);
-			}
-		}
-	}
-}
-
-void Ekf::controlDragFusion()
-{
-	if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
-	    !_control_status.flags.fake_pos && _control_status.flags.in_air) {
-
-		if (!_control_status.flags.wind) {
-			// reset the wind states and covariances when starting drag accel fusion
-			_control_status.flags.wind = true;
-			resetWind();
-
-		}
-
-
-		dragSample drag_sample;
-
-		if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) {
-			fuseDrag(drag_sample);
-		}
-	}
-}
-
-void Ekf::controlAuxVelFusion()
-{
-	if (_auxvel_buffer) {
-		auxVelSample auxvel_sample_delayed;
-
-		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) {
-
-			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);
-
-			if (isHorizontalAidingActive()) {
-				_aid_src_aux_vel.fusion_enabled = true;
-				fuseVelocity(_aid_src_aux_vel);
-			}
-		}
-	}
-}
@@ -605,34 +605,3 @@ void Ekf::resetZDeltaAngBiasCov()

 	P.uncorrelateCovarianceSetVariance<1>(12, init_delta_ang_bias_var);
 }
-
-void Ekf::resetWindCovarianceUsingAirspeed()
-{
-	// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
-	// TODO: explicitly include the sideslip angle in the derivation
-	const float euler_yaw = getEulerYaw(_R_to_earth);
-	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
-	constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
-	const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
-	constexpr float R_yaw = sq(math::radians(10.0f));
-
-	const float cos_yaw = cosf(euler_yaw);
-	const float sin_yaw = sinf(euler_yaw);
-
-	// rotate wind velocity into earth frame aligned with vehicle yaw
-	const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw;
-	const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;
-
-	// it is safer to remove all existing correlations to other states at this time
-	P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
-
-	P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
-	P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
-		    initial_wind_var_body_y * sin_yaw * cos_yaw;
-	P(23, 22) = P(22, 23);
-	P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
-
-	// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
-	P(22, 22) += P(4, 4);
-	P(23, 23) += P(5, 5);
-}
@@ -42,6 +42,25 @@

 #include <mathlib/mathlib.h>

+void Ekf::controlDragFusion()
+{
+	if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
+	    !_control_status.flags.fake_pos && _control_status.flags.in_air) {
+
+		if (!_control_status.flags.wind) {
+			// reset the wind states and covariances when starting drag accel fusion
+			_control_status.flags.wind = true;
+			resetWindToZero();
+		}
+
+		dragSample drag_sample;
+
+		if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) {
+			fuseDrag(drag_sample);
+		}
+	}
+}
+
 void Ekf::fuseDrag(const dragSample &drag_sample)
 {
 	const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
@@ -488,13 +488,9 @@ private:

 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
-	bool _rng_data_ready{false};
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
-	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator

-	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)
-
 	uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
 	uint64_t _time_last_v_pos_aiding{0};
 	uint64_t _time_last_v_vel_aiding{0};
@@ -682,8 +678,8 @@ private:
 	// apply sensible limits to the declination and length of the NE mag field states estimates
 	void limitDeclination();

-	void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const;
-	void fuseAirspeed(estimator_aid_source1d_s &airspeed);
+	void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const;
+	void fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src);

 	// fuse synthetic zero sideslip measurement
 	void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
@@ -858,7 +854,7 @@ private:
 	void resetOnGroundMotionForOpticalFlowChecks();

 	// control fusion of GPS observations
-	void controlGpsFusion();
+	void controlGpsFusion(const imuSample &imu_delayed);
 	bool shouldResetGpsFusion() const;
 	bool isYawFailure() const;

@@ -887,7 +883,7 @@ private:
 	void run3DMagAndDeclFusions(const Vector3f &mag);

 	// control fusion of air data observations
-	void controlAirDataFusion();
+	void controlAirDataFusion(const imuSample &imu_delayed);

 	// control fusion of synthetic sideslip observations
 	void controlBetaFusion(const imuSample &imu_delayed);
@@ -952,12 +948,15 @@ private:
 	void zeroQuatCov();
 	void resetMagCov();

-	// perform a limited reset of the wind state covariances
-	void resetWindCovarianceUsingAirspeed();
-
 	// perform a reset of the wind states and related covariances
 	void resetWind();
-	void resetWindUsingAirspeed();
+
+	// 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);
+
+	// perform a limited reset of the wind state covariances
+	void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample);
+
 	void resetWindToZero();

 	// check that the range finder data is continuous
@@ -997,7 +996,6 @@ private:
 		return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us);
 	}

-	void startAirspeedFusion();
 	void stopAirspeedFusion();

 	void stopGpsFusion();
@@ -1038,8 +1036,6 @@ private:
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};

-	void runYawEKFGSF(const imuSample &imu_delayed);
-
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
 	// Resets the horizontal velocity and position to the default navigation sensor
 	// Returns true if the reset was successful
@@ -212,79 +212,6 @@ void Ekf::resetVerticalVelocityToZero()
 	resetVerticalVelocityTo(0.0f, 10.f);
 }

-// Reset heading and magnetic field states
-bool Ekf::resetMagHeading()
-{
-	// prevent a reset being performed more than once on the same frame
-	if ((_flt_mag_align_start_time == _time_delayed_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
-		return false;
-	}
-
-	const Vector3f mag_init = _mag_lpf.getState();
-
-	const bool mag_available = (_mag_counter > 1) && !magFieldStrengthDisturbed(mag_init);
-
-	// low pass filtered mag required
-	if (!mag_available) {
-		return false;
-	}
-
-	const bool heading_required_for_navigation = _control_status.flags.gps;
-
-	if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {
-
-		// rotate the magnetometer measurements into earth frame using a zero yaw angle
-		const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
-
-		// the angle of the projection onto the horizontal gives the yaw angle
-		const Vector3f mag_earth_pred = R_to_earth * mag_init;
-
-		// calculate the observed yaw angle and yaw variance
-		float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
-		float yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.e-2f));
-
-		ECL_INFO("reset mag heading %.3f -> %.3f rad (declination %.1f)", (double)getEulerYaw(_R_to_earth), (double)yaw_new, (double)getMagDeclination());
-
-		// update quaternion states and corresponding covarainces
-		resetQuatStateYaw(yaw_new, yaw_new_variance);
-
-		// set the earth magnetic field states using the updated rotation
-		_state.mag_I = _R_to_earth * mag_init;
-
-		resetMagCov();
-
-		// record the time for the magnetic field alignment event
-		_flt_mag_align_start_time = _time_delayed_us;
-
-		return true;
-	}
-
-	return false;
-}
-
-// Return the magnetic declination in radians to be used by the alignment and fusion processing
-float Ekf::getMagDeclination()
-{
-	// set source of magnetic declination for internal use
-	if (_control_status.flags.mag_aligned_in_flight) {
-		// Use value consistent with earth field state
-		return atan2f(_state.mag_I(1), _state.mag_I(0));
-
-	} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
-		// use parameter value until GPS is available, then use value returned by geo library
-		if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
-			return _mag_declination_gps;
-
-		} else {
-			return math::radians(_params.mag_declination_deg);
-		}
-
-	} else {
-		// always use the parameter value
-		return math::radians(_params.mag_declination_deg);
-	}
-}
-
 void Ekf::constrainStates()
 {
 	_state.quat_nominal = matrix::constrain(_state.quat_nominal, -1.0f, 1.0f);
@@ -1033,63 +960,6 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 	}
 }

-void Ekf::stopMagFusion()
-{
-	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		ECL_INFO("stopping all mag fusion");
-		stopMag3DFusion();
-		stopMagHdgFusion();
-		clearMagCov();
-	}
-}
-
-void Ekf::stopMag3DFusion()
-{
-	// save covariance data for re-use if currently doing 3-axis fusion
-	if (_control_status.flags.mag_3D) {
-		saveMagCovData();
-
-		_control_status.flags.mag_3D = false;
-		_control_status.flags.mag_dec = false;
-
-		_fault_status.flags.bad_mag_x = false;
-		_fault_status.flags.bad_mag_y = false;
-		_fault_status.flags.bad_mag_z = false;
-
-		_fault_status.flags.bad_mag_decl = false;
-	}
-}
-
-void Ekf::stopMagHdgFusion()
-{
-	if (_control_status.flags.mag_hdg) {
-		_control_status.flags.mag_hdg = false;
-
-		_fault_status.flags.bad_hdg = false;
-	}
-}
-
-void Ekf::startMagHdgFusion()
-{
-	if (!_control_status.flags.mag_hdg) {
-		stopMag3DFusion();
-		ECL_INFO("starting mag heading fusion");
-		_control_status.flags.mag_hdg = true;
-	}
-}
-
-void Ekf::startMag3DFusion()
-{
-	if (!_control_status.flags.mag_3D) {
-
-		stopMagHdgFusion();
-
-		zeroMagCov();
-		loadMagCovData();
-		_control_status.flags.mag_3D = true;
-	}
-}
-
 void Ekf::updateGroundEffect()
 {
 	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
@@ -1181,105 +1051,6 @@ void Ekf::loadMagCovData()
 	P(18, 18) = _saved_mag_ef_d_variance;
 }

-void Ekf::startAirspeedFusion()
-{
-	if (!_control_status.flags.fuse_aspd) {
-		ECL_INFO("starting airspeed fusion");
-
-		// If starting wind state estimation, reset the wind states and covariances before fusing any data
-		if (!_control_status.flags.wind) {
-			// activate the wind states
-			_control_status.flags.wind = true;
-			// reset the wind speed states and corresponding covariances
-			resetWindUsingAirspeed();
-		}
-
-		_control_status.flags.fuse_aspd = true;
-	}
-}
-
-void Ekf::stopAirspeedFusion()
-{
-	if (_control_status.flags.fuse_aspd) {
-		ECL_INFO("stopping airspeed fusion");
-		_control_status.flags.fuse_aspd = false;
-	}
-}
-
-void Ekf::stopGpsFusion()
-{
-	if (_control_status.flags.gps) {
-		stopGpsPosFusion();
-		stopGpsVelFusion();
-
-		_control_status.flags.gps = false;
-	}
-
-	if (_control_status.flags.gps_yaw) {
-		stopGpsYawFusion();
-	}
-
-	// We do not need to know the true North anymore
-	// EV yaw can start again
-	_inhibit_ev_yaw_use = false;
-}
-
-void Ekf::stopGpsPosFusion()
-{
-	if (_control_status.flags.gps) {
-		ECL_INFO("stopping GPS position fusion");
-		_control_status.flags.gps = false;
-
-		resetEstimatorAidStatus(_aid_src_gnss_pos);
-	}
-}
-
-void Ekf::stopGpsVelFusion()
-{
-	ECL_INFO("stopping GPS velocity fusion");
-
-	resetEstimatorAidStatus(_aid_src_gnss_vel);
-}
-
-void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
-{
-	if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
-		ECL_INFO("starting GPS yaw fusion");
-		_control_status.flags.yaw_align = true;
-		_control_status.flags.mag_dec = false;
-		stopEvYawFusion();
-		stopMagHdgFusion();
-		stopMag3DFusion();
-		_control_status.flags.gps_yaw = true;
-	}
-}
-
-void Ekf::stopGpsYawFusion()
-{
-	if (_control_status.flags.gps_yaw) {
-		ECL_INFO("stopping GPS yaw fusion");
-		_control_status.flags.gps_yaw = false;
-		resetEstimatorAidStatus(_aid_src_gnss_yaw);
-	}
-}
-
-void Ekf::stopAuxVelFusion()
-{
-	ECL_INFO("stopping aux vel fusion");
-	//_control_status.flags.aux_vel = false;
-	resetEstimatorAidStatus(_aid_src_aux_vel);
-}
-
-void Ekf::stopFlowFusion()
-{
-	if (_control_status.flags.opt_flow) {
-		ECL_INFO("stopping optical flow fusion");
-		_control_status.flags.opt_flow = false;
-
-		resetEstimatorAidStatus(_aid_src_optical_flow);
-	}
-}
-
 void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 {
 	// save a copy of the quaternion state for later use in calculating the amount of reset change
@@ -1371,23 +1142,6 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
 	return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
 }

-void Ekf::runYawEKFGSF(const imuSample &imu_delayed)
-{
-	float TAS = 0.f;
-
-	if (_control_status.flags.fixed_wing) {
-		if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000)) {
-			TAS = _params.EKFGSF_tas_default;
-
-		} else if (_airspeed_sample_delayed.true_airspeed >= _params.arsp_thr) {
-			TAS = _airspeed_sample_delayed.true_airspeed;
-		}
-	}
-
-	const Vector3f imu_gyro_bias = getGyroBias();
-	_yawEstimator.update(imu_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
-}
-
 void Ekf::resetGpsDriftCheckFilters()
 {
 	_gps_velNE_filt.setZero();
@@ -105,7 +105,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 					// Data seems good, attempt a reset
 					//_information_events.flags.reset_yaw_to_vision = true; // TODO
 					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
-					resetQuatStateYaw(aid_src.innovation, aid_src.observation_variance);
+					resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
 					aid_src.time_last_fuse = _time_delayed_us;

 					if (_control_status.flags.in_air) {
@@ -39,13 +39,41 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>

-void Ekf::controlGpsFusion()
+void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 {
-	if (!((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
+	if (!_gps_buffer || !((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
 		stopGpsFusion();
 		return;
 	}

+	_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
+
+	// check for arrival of new sensor data at the fusion time horizon
+	_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
+
+	if (_gps_data_ready) {
+		// correct velocity for offset relative to IMU
+		const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+		const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+		_gps_sample_delayed.vel -= vel_offset_earth;
+
+		// correct position and height for offset relative to IMU
+		const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+		_gps_sample_delayed.pos -= pos_offset_earth.xy();
+		_gps_sample_delayed.hgt += pos_offset_earth(2);
+
+		// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
+		if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc)
+		    && _gps_sample_delayed.vel.isAllFinite()
+		   ) {
+			_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise));
+		}
+	}
+
+	// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
+	_yawEstimator.update(imu_delayed, _control_status.flags.in_air, getGyroBias());
+
 	// Check for new GPS data that has fallen behind the fusion time horizon
 	if (_gps_data_ready) {

@@ -236,8 +264,8 @@ bool Ekf::shouldResetGpsFusion() const
 	 * with no aiding we need to do something
 	 */
 	const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-						     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-						     && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
+			&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+			&& isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);

 	const bool is_reset_required = has_horizontal_aiding_timed_out
 				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
@@ -262,3 +290,150 @@ bool Ekf::isYawFailure() const

 	return fabsf(yaw_error) > math::radians(25.f);
 }
+
+void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
+{
+	if (!(_params.gnss_ctrl & GnssCtrl::YAW)
+	    || _control_status.flags.gps_yaw_fault) {
+
+		stopGpsYawFusion();
+		return;
+	}
+
+	updateGpsYaw(gps_sample);
+
+	const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
+
+	if (is_new_data_available) {
+
+		const bool continuing_conditions_passing = !gps_checks_failing;
+
+		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push,
+				2 * GNSS_YAW_MAX_INTERVAL);
+
+		const bool starting_conditions_passing = continuing_conditions_passing
+				&& _control_status.flags.tilt_align
+				&& gps_checks_passing
+				&& !is_gps_yaw_data_intermittent
+				&& !_gps_intermittent;
+
+		if (_control_status.flags.gps_yaw) {
+
+			if (continuing_conditions_passing) {
+
+				fuseGpsYaw();
+
+				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
+
+				if (is_fusion_failing) {
+					if (_nb_gps_yaw_reset_available > 0) {
+						// Data seems good, attempt a reset
+						resetYawToGps(gps_sample.yaw);
+
+						if (_control_status.flags.in_air) {
+							_nb_gps_yaw_reset_available--;
+						}
+
+					} else if (starting_conditions_passing) {
+						// Data seems good, but previous reset did not fix the issue
+						// something else must be wrong, declare the sensor faulty and stop the fusion
+						_control_status.flags.gps_yaw_fault = true;
+						stopGpsYawFusion();
+
+					} else {
+						// A reset did not fix the issue but all the starting checks are not passing
+						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+						stopGpsYawFusion();
+					}
+
+					// TODO: should we give a new reset credit when the fusion does not fail for some time?
+				}
+
+			} else {
+				// Stop GPS yaw fusion but do not declare it faulty
+				stopGpsYawFusion();
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				// Try to activate GPS yaw fusion
+				startGpsYawFusion(gps_sample);
+
+				if (_control_status.flags.gps_yaw) {
+					_nb_gps_yaw_reset_available = 1;
+				}
+			}
+		}
+
+	} else if (_control_status.flags.gps_yaw
+		   && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
+
+		// No yaw data in the message anymore. Stop until it comes back.
+		stopGpsYawFusion();
+	}
+
+	// Before takeoff, we do not want to continue to rely on the current heading
+	// if we had to stop the fusion
+	if (!_control_status.flags.in_air
+	    && !_control_status.flags.gps_yaw
+	    && _control_status_prev.flags.gps_yaw) {
+		_control_status.flags.yaw_align = false;
+	}
+}
+
+void Ekf::stopGpsFusion()
+{
+	if (_control_status.flags.gps) {
+		stopGpsPosFusion();
+		stopGpsVelFusion();
+
+		_control_status.flags.gps = false;
+	}
+
+	if (_control_status.flags.gps_yaw) {
+		stopGpsYawFusion();
+	}
+
+	// We do not need to know the true North anymore
+	// EV yaw can start again
+	_inhibit_ev_yaw_use = false;
+}
+
+void Ekf::stopGpsPosFusion()
+{
+	if (_control_status.flags.gps) {
+		ECL_INFO("stopping GPS position fusion");
+		_control_status.flags.gps = false;
+
+		resetEstimatorAidStatus(_aid_src_gnss_pos);
+	}
+}
+
+void Ekf::stopGpsVelFusion()
+{
+	ECL_INFO("stopping GPS velocity fusion");
+
+	resetEstimatorAidStatus(_aid_src_gnss_vel);
+}
+
+void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
+{
+	if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
+		ECL_INFO("starting GPS yaw fusion");
+		_control_status.flags.yaw_align = true;
+		_control_status.flags.mag_dec = false;
+		stopEvYawFusion();
+		stopMagHdgFusion();
+		stopMag3DFusion();
+		_control_status.flags.gps_yaw = true;
+	}
+}
+
+void Ekf::stopGpsYawFusion()
+{
+	if (_control_status.flags.gps_yaw) {
+		ECL_INFO("stopping GPS yaw fusion");
+		_control_status.flags.gps_yaw = false;
+		resetEstimatorAidStatus(_aid_src_gnss_yaw);
+	}
+}
@@ -107,6 +107,10 @@ void Ekf::controlMagFusion()
 			_aid_src_mag.timestamp_sample = mag_sample.time_us;
 			mag_observation.copyTo(_aid_src_mag.observation);
 			mag_innov.copyTo(_aid_src_mag.innovation);
+
+		} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
+			// No data anymore. Stop until it comes back.
+			stopMagFusion();
 		}
 	}

@@ -472,3 +476,131 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		}
 	}
 }
+
+bool Ekf::resetMagHeading()
+{
+	// prevent a reset being performed more than once on the same frame
+	if ((_flt_mag_align_start_time == _time_delayed_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
+		return false;
+	}
+
+	const Vector3f mag_init = _mag_lpf.getState();
+
+	const bool mag_available = (_mag_counter > 1) && !magFieldStrengthDisturbed(mag_init);
+
+	// low pass filtered mag required
+	if (!mag_available) {
+		return false;
+	}
+
+	const bool heading_required_for_navigation = _control_status.flags.gps;
+
+	if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {
+
+		// rotate the magnetometer measurements into earth frame using a zero yaw angle
+		const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
+
+		// the angle of the projection onto the horizontal gives the yaw angle
+		const Vector3f mag_earth_pred = R_to_earth * mag_init;
+
+		// calculate the observed yaw angle and yaw variance
+		float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+		float yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.e-2f));
+
+		ECL_INFO("reset mag heading %.3f -> %.3f rad (declination %.1f)", (double)getEulerYaw(_R_to_earth), (double)yaw_new, (double)getMagDeclination());
+
+		// update quaternion states and corresponding covarainces
+		resetQuatStateYaw(yaw_new, yaw_new_variance);
+
+		// set the earth magnetic field states using the updated rotation
+		_state.mag_I = _R_to_earth * mag_init;
+
+		resetMagCov();
+
+		// record the time for the magnetic field alignment event
+		_flt_mag_align_start_time = _time_delayed_us;
+
+		return true;
+	}
+
+	return false;
+}
+
+float Ekf::getMagDeclination()
+{
+	// set source of magnetic declination for internal use
+	if (_control_status.flags.mag_aligned_in_flight) {
+		// Use value consistent with earth field state
+		return atan2f(_state.mag_I(1), _state.mag_I(0));
+
+	} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
+		// use parameter value until GPS is available, then use value returned by geo library
+		if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
+			return _mag_declination_gps;
+
+		} else {
+			return math::radians(_params.mag_declination_deg);
+		}
+
+	} else {
+		// always use the parameter value
+		return math::radians(_params.mag_declination_deg);
+	}
+}
+
+void Ekf::stopMagFusion()
+{
+	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+		ECL_INFO("stopping all mag fusion");
+		stopMag3DFusion();
+		stopMagHdgFusion();
+		clearMagCov();
+	}
+}
+
+void Ekf::stopMag3DFusion()
+{
+	// save covariance data for re-use if currently doing 3-axis fusion
+	if (_control_status.flags.mag_3D) {
+		saveMagCovData();
+
+		_control_status.flags.mag_3D = false;
+		_control_status.flags.mag_dec = false;
+
+		_fault_status.flags.bad_mag_x = false;
+		_fault_status.flags.bad_mag_y = false;
+		_fault_status.flags.bad_mag_z = false;
+
+		_fault_status.flags.bad_mag_decl = false;
+	}
+}
+
+void Ekf::stopMagHdgFusion()
+{
+	if (_control_status.flags.mag_hdg) {
+		_control_status.flags.mag_hdg = false;
+
+		_fault_status.flags.bad_hdg = false;
+	}
+}
+
+void Ekf::startMagHdgFusion()
+{
+	if (!_control_status.flags.mag_hdg) {
+		stopMag3DFusion();
+		ECL_INFO("starting mag heading fusion");
+		_control_status.flags.mag_hdg = true;
+	}
+}
+
+void Ekf::startMag3DFusion()
+{
+	if (!_control_status.flags.mag_3D) {
+
+		stopMagHdgFusion();
+
+		zeroMagCov();
+		loadMagCovData();
+		_control_status.flags.mag_3D = true;
+	}
+}
@@ -40,6 +40,15 @@

 void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 {
+	if (_flow_buffer) {
+		// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
+		// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
+		// in this case we need to empty the buffer
+		if (!_flow_data_ready || (!_control_status.flags.opt_flow && !_hagl_sensor_status.flags.flow)) {
+			_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
+		}
+	}
+
 	// Check if on ground motion is un-suitable for use of optical flow
 	if (!_control_status.flags.in_air) {
 		updateOnGroundMotionForOpticalFlowChecks();
@@ -249,3 +258,13 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
 	_time_bad_motion_us = 0;
 	_time_good_motion_us = _time_delayed_us;
 }
+
+void Ekf::stopFlowFusion()
+{
+	if (_control_status.flags.opt_flow) {
+		ECL_INFO("stopping optical flow fusion");
+		_control_status.flags.opt_flow = false;
+
+		resetEstimatorAidStatus(_aid_src_optical_flow);
+	}
+}
@@ -42,12 +42,61 @@ void Ekf::controlRangeHeightFusion()
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";

+	bool rng_data_ready = false;
+
+	if (_range_buffer) {
+		// Get range data from buffer and check validity
+		rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress());
+		_range_sensor.setDataReadiness(rng_data_ready);
+
+		// update range sensor angle parameters in case they have changed
+		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
+		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+
+		_range_sensor.runChecks(_time_delayed_us, _R_to_earth);
+
+		if (_range_sensor.isDataHealthy()) {
+			// correct the range data for position offset relative to the IMU
+			const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
+
+			// Run the kinematic consistency check when not moving horizontally
+			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
+			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
+
+				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+				const float var = sq(_params.range_noise) + dist_dependant_var;
+
+				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us);
+			}
+
+		} else {
+			// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
+			// and are on the ground, then synthesise a measurement at the expected on ground value
+			if (!_control_status.flags.in_air
+			    && _range_sensor.isRegularlySendingData()
+			    && _range_sensor.isDataReady()) {
+
+				_range_sensor.setRange(_params.rng_gnd_clearance);
+				_range_sensor.setValidity(true); // bypass the checks
+			}
+		}
+
+		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	} else {
+		return;
+	}
+
 	auto &aid_src = _aid_src_rng_hgt;
 	HeightBiasEstimator &bias_est = _rng_hgt_b_est;

 	bias_est.predict(_dt_ekf_avg);

-	if (_rng_data_ready && _range_sensor.getSampleAddress()) {
+	if (rng_data_ready && _range_sensor.getSampleAddress()) {

 		const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
 		const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
@@ -47,6 +47,37 @@

 #include <mathlib/mathlib.h>

+void Ekf::controlBetaFusion(const imuSample &imu_delayed)
+{
+	_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
+		&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
+
+	if (_control_status.flags.fuse_beta) {
+
+		// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally:
+		const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
+
+		if (beta_fusion_time_triggered) {
+
+			updateSideslip(_aid_src_sideslip);
+			_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
+
+			// If starting wind state estimation, reset the wind states and covariances before fusing any data
+			if (!_control_status.flags.wind) {
+				// activate the wind states
+				_control_status.flags.wind = true;
+				// reset the timeout timers to prevent repeated resets
+				_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
+				resetWindToZero();
+			}
+
+			if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
+				fuseSideslip(_aid_src_sideslip);
+			}
+		}
+	}
+}
+
 void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
 {
 	// reset flags
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -240,15 +240,15 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	// User input assisted landing
 	if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
 		// Stick full up -1 -> stop, stick full down 1 -> double the speed
-		vertical_speed *= (1 + _sticks.getPositionExpo()(2));
+		vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo());

 		// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
-		if (!_weathervane.isActive() || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
-			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
-						       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
+		if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
+			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
+						       _deltatime);
 		}

-		_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
+		_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _land_heading, _position,
 				_velocity_setpoint_feedback.xy(), _deltatime);
 		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -142,7 +142,7 @@ protected:
 	Vector3f _unsmoothed_velocity_setpoint;
 	Sticks _sticks{this};
 	StickAccelerationXY _stick_acceleration_xy{this};
-	StickYaw _stick_yaw;
+	StickYaw _stick_yaw{this};
 	matrix::Vector3f _land_position;
 	float _land_heading;
 	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
@@ -176,8 +176,7 @@ protected:
 					(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
 					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
 					(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
-					_param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp
-					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
+					_param_mpc_tko_ramp_t // time constant for smooth takeoff ramp
 				       );

 private:
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -35,5 +35,5 @@ px4_add_library(FlightTaskDescend
 	FlightTaskDescend.cpp
 )

-target_link_libraries(FlightTaskDescend PUBLIC FlightTask)
+target_link_libraries(FlightTaskDescend PUBLIC FlightTask FlightTaskUtility)
 target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -36,16 +36,6 @@

 #include "FlightTaskDescend.hpp"

-bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint)
-{
-	bool ret = FlightTask::activate(last_setpoint);
-	// stay level to minimize horizontal drift
-	_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN);
-	// keep heading
-	_yaw_setpoint = _yaw;
-	return ret;
-}
-
 bool FlightTaskDescend::update()
 {
 	bool ret = FlightTask::update();
@@ -58,7 +48,28 @@ bool FlightTaskDescend::update()
 	} else {
 		// descend with constant acceleration (crash landing)
 		_velocity_setpoint(2) = NAN;
-		_acceleration_setpoint(2) = .3f;
+		_acceleration_setpoint(2) = .15f;
+	}
+
+	// Nudging
+	if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
+		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
+					       _is_yaw_good_for_control, _deltatime);
+		_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
+					      _yaw_setpoint);
+
+		// Stick full up -1 -> stop, stick full down 1 -> double the value
+		_velocity_setpoint(2) *= (1 - _sticks.getThrottleZeroCenteredExpo());
+		_acceleration_setpoint(2) -= _sticks.getThrottleZeroCentered() * 10.f;
+
+	} else {
+		_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN); // stay level to minimize horizontal drift
+		_yawspeed_setpoint = NAN;
+
+		// keep heading
+		if (!PX4_ISFINITE(_yaw_setpoint)) {
+			_yaw_setpoint = _yaw;
+		}
 	}

 	return ret;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -38,6 +38,9 @@
 #pragma once

 #include "FlightTask.hpp"
+#include "Sticks.hpp"
+#include "StickTiltXY.hpp"
+#include "StickYaw.hpp"

 class FlightTaskDescend : public FlightTask
 {
@@ -46,11 +49,15 @@ public:
 	virtual ~FlightTaskDescend() = default;

 	bool update() override;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;

 private:
+	Sticks _sticks{this};
+	StickTiltXY _stick_tilt_xy{this};
+	StickYaw _stick_yaw{this};
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-					(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, ///< thrust at hover equilibrium
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed ///< velocity for controlled descend
+					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
+					(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover ///< thrust at hover equilibrium
 				       )
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -63,11 +63,10 @@ bool FlightTaskManualAcceleration::update()
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();

-	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
-				       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
+	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
+				       _deltatime);

-	_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint,
-			_position,
+	_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,
 			_velocity_setpoint_feedback.xy(), _deltatime);
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -58,7 +58,7 @@ private:
 	void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;

 	StickAccelerationXY _stick_acceleration_xy{this};
-	StickYaw _stick_yaw;
+	StickYaw _stick_yaw{this};

 	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 };
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Matthias Grob	5c9fac79ae	Vector2: Add function to rotate a 2D vector	2023-03-03 19:03:00 +01:00
Silvan Fuhrer	7edce94b93	v2_default: disable hover thrust estimator to safe flash Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-02 12:22:18 -05:00
Daniel Agar	74130d7f71	ucdr msg template add timestamp protections - if timestamp (or timestamp_sample) unpopulated fill it with current hrt_absolute_time() - if using provided timestamp don't allow it to exceed current hrt_absolute_time()	2023-03-02 09:52:53 -05:00
Matthias Grob	0770478dc7	FlightTaskDescend: Enable nudging by sticks when it's enabled and stick input is available.	2023-03-02 12:06:40 +01:00
Matthias Grob	728570828f	FlightTaskManualAltitude: use StickTiltXY for horizontal stick mapping	2023-03-02 12:06:40 +01:00
Matthias Grob	21d580293a	StickYaw: use consistently for all flight tasks - Switching to the first order filter that was previously only in FlightTaskManualAltitude. - Moving the scaling of full stick deflection to radians per second into the class.	2023-03-02 12:06:40 +01:00
Matthias Grob	0c1f340154	StickAccelerationXY: improve comments	2023-03-02 12:06:40 +01:00
Matthias Grob	a29d02fd62	MulticopterRateControl: don't instaciate unused parameter	2023-03-02 12:06:40 +01:00
Matthias Grob	da4644c20a	Sticks: only use stick input if flagged valid and add a function for just pitch roll stick input	2023-03-02 12:06:40 +01:00
Matthias Grob	1dada5daf4	Add StickTiltXY utility class to FLightTasks to map stick input to vehicle tilt consistently and reliably across modes.	2023-03-02 12:06:40 +01:00
Silvan Fuhrer	0c4b288973	RTL: only do calculations in is-inactive if global position is recent (#21208 ) * RTL: only do calculations in is-inactive if global position is recent * RTL: refactor calcRtlTimeEstimate to only calc and not pub Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-02 11:09:32 +01:00
Beniamino Pozzan	458c351585	setup/ubuntu.sh: Only install Gazebo Garden for Ubuntu 22.04 (#21173 ) * Tools/setup/ubuntu.sh: Only install Gazebo Garden on Ubuntu 22.04 Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it> * Apply suggestions from code review --------- Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it> Co-authored-by: Hamish Willee <hamishwillee@gmail.com>	2023-03-02 10:51:29 +11:00
Andrew Brahim	184c7fe79d	drivers/distance_sensor: Lightware Lidar SF45/B rotating sensor serial driver (#19891 ) Signed-off-by: dirksavage88 <dirksavage88@gmail.com>	2023-03-01 16:16:25 -05:00
Daniel Agar	bde194fb12	simulation/gz_bridge: local_position_groundtruth include heading (#21224 )	2023-03-01 18:44:31 +01:00
Beat Küng	e129534a58	fix ROMFS: add rc.autostart_ext to cmake	2023-03-01 08:53:17 -05:00
Daniel Mesham	fe48de6240	Check position subscriber before force-send flag when sending GPS global origin stream	2023-03-01 08:52:41 -05:00
DanielePettenuzzo	539f874325	mavlink main - enable gps global origin stream also on mavlink low bw mode and change all rates to 1Hz	2023-03-01 08:52:41 -05:00
DanielePettenuzzo	6bf19ebe23	gps global origin stream - make sure we can always send out the message at least once on request When requesting a message from a stream that is not active we start the stream with interval=0 and call the request method once. For all streams this works fine except the gps_global_origin. For this one the request method is actually overidden to throttle down the rate and not just send out the message. This will cause this message to never being sent on request if the stream is not active by default.	2023-03-01 08:52:41 -05:00
Silvan Fuhrer	76116d79f9	TECS: remove umcommanded_descent flag Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-01 10:39:09 +01:00
Silvan Fuhrer	527225357b	TECS: remove unused TECS_MODE_CLIMBOUT Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-01 10:39:09 +01:00
Tony Samaritano	2b73b6df70	commander: fixes valid mag count in HIL	2023-03-01 08:05:23 +01:00
Junwoo Hwang	d6b523b574	Update README: Maintainers, Boards, Roadmap (#21030 ) * Update README: Maintainers, Boards, Roadmap - First step after the community coordination call from January 30th * README: Fromat list & remove discontinued boards & add others - Addressed comments * README: Add Simulation, remove QGC - Only leave the PX4 specific categories (QGC is not) * Add Beniamino as ROS2 maintainer * README: Add note that README is main source of truth for maintainers - We need to have a source of truth, we can use Github README for that.	2023-02-28 19:06:09 +01:00
Marco Räth	bdb0fe77d0	v6x: fix mag orientation for V6X009010 and V6X010010 (#21194 )	2023-02-28 11:01:16 +01:00
João Neto	58000ff61c	Tools/setup/ubuntu.sh: remove comment from continued line (#21191 ) Comment broke script	2023-02-28 08:12:20 +01:00
Daniel Agar	b5a6d6db0d	ekf2: fix controlEvYawFusion() yaw reset	2023-02-24 16:59:59 -05:00
Daniel Agar	a06a635da3	drivers/inv/vectornav: fix official vectornav library NuttX support - vectornav library (libvnc) fixed for NuttX - open serial port O_NONBLOCK (like __APPLE__) - set serial port baud rate with cfsetspeed (like __APPLE__) - vectornav backend thread increase stack and run at higher priority (SCHED_FIFO)	2023-02-24 16:59:38 -05:00
Silvan Fuhrer	837095b9a8	tecs: use FW_T_SINK_MIN for STE_rate_min (#21190 ) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-24 15:46:41 +01:00
Roman Bapst	33b54f7c57	vtol_att_control: Consolidate logic for front transiton completion (#21107 ) * vtol_att_control: consolidate logic for front transiton completion Signed-off-by: RomanBapst <bapstroman@gmail.com> * addressed review comments Signed-off-by: RomanBapst <bapstroman@gmail.com> --------- Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-24 15:52:27 +03:00
Silvan Fuhrer	4259b5adac	Commander: copy sensor_gps in HomePosition::update() and store relevant fields in separate variables Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-24 13:32:38 +01:00
Silvan Fuhrer	006321e278	Commander: remove unused param COM_POS_FS_EPV Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-24 13:32:38 +01:00
Silvan Fuhrer	526e066d9a	Commander: rework GPS invalid warning to use estimator feedback instead of separate GPS quality thresholds Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-24 13:32:38 +01:00
Silvan Fuhrer	e6af8b9aa6	Commander: Home Position: move gps checks on when to allow setting home pos inside HomePosition class Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-24 13:32:38 +01:00
Igor Mišić	352f773ec4	systemcmds/mtd: fix rwtest - force data to/from the device Block Device driver uses a buffer so we need to ensure data is written or read to the device and not to the buffer so we can be sure if the device works properly	2023-02-24 08:08:19 +01:00
Dmitry Ponomarev	a1efafc42b	drivers/cyphal: incremental fixes for fmu-v5 (#20671 ) * Cyphal: fix comparing floating-point issue * Cyphal: fix setpoint serialization * Cyphal: fix bug with wrong comparasion of param name and pub/sub name: remove prefix from UavcanPublisher::updateParam and UavcanDynamicPortSubscriber::updateParam and PublicationManager::updateDynamicPublications * Cyphal: integrate UavcanEscController with PublicationManager, remove second instance of UavcanEscController from CyphalNode * Cyphal: publish readiness with minimal frequency because according to UDRAL The drive shall enter STANDBY state automatically if the readiness subject is not updated for CONTROL_TIMEOUT * Cyphal: increase setpoint publish rate from ~75 to 200 by removing PX4_INFO (it really significantly react on the the output rate) and changing the mixing output rate and the shedule interval * Cyphal: restore prefix because we need it for uorb over uavcan/cyphal and add udral prefix for non uorb pub/sub * Cyphal: fix DynamicPortSubscriber subscription: if it has multiple subscribers, it should call subscription only after updating of all port subscribers port identifiers * Cyphal: fix SubscriptionManager: we should take care about prefix * Cyphal: fix readiness for test motor mode * [Cyphal] Fix dynamicsubscription, improve printinfo, enable MR-CANHUBK3 config --------- Co-authored-by: Peter van der Perk <peter.vanderperk@nxp.com>	2023-02-23 10:57:50 -05:00
Matthias Grob	013365d6c8	ubuntu.sh: source the .profile after changing it such that the arm toolchain is available in that terminal without relogin for convenience.	2023-02-23 09:58:10 +01:00
Matthias Grob	22030c1b8c	ubuntu.sh: add libfuse2 to general packages to allow running QGroundControl app image out of the box.	2023-02-23 09:58:10 +01:00
Matthias Grob	dabf33759b	ubuntu.sh: only add gazebo source once on 22.04	2023-02-23 09:58:10 +01:00
Hamish Willee	76aac7a5e5	VehicleCommandAck typo on module docs	2023-02-23 08:19:11 +01:00
Hamish Willee	4daa63afc2	Sensors.cpp - case the uorb topics like SensorGyro	2023-02-23 08:18:39 +01:00
Alejandro Hernández Cordero	ea6814d258	Simulation Gazebo: Use Gazebo Airpressure sensor (#21176 ) * Simulation Gazebo: Use Gazebo Airpressure sensor Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * Fixed build Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * Added feedback Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * make linters happy Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> --------- Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>	2023-02-22 19:32:25 +01:00
Beat Küng	06dfd1726f	microdds_client: fix -l flag and add -c for custom participant configuration Allows to use a custom FastDDS configuration on the Agent side.	2023-02-22 11:15:29 -05:00
Daniel Agar	98263de17b	ekf2: move aux vel helpers to auxvel_fusion.cpp	2023-02-22 09:08:33 -05:00
Daniel Agar	a199df78cc	ekf2: move mag control helpers to mag_control.cpp	2023-02-22 09:08:33 -05:00
Daniel Agar	c20e4e4421	ekf2: move stopFlowFusion() to optical_flow_control.cpp	2023-02-22 09:08:33 -05:00
Daniel Agar	3a317ec18c	ekf2: move gps helpers to gps_control.cpp	2023-02-22 09:08:33 -05:00
Daniel Agar	9ec3f30ae1	ekf2: move gps buffer pop to controlGpsFusion() - controlGpsFusion() now owns yaw estimator update	2023-02-22 09:08:33 -05:00
Daniel Agar	a867bb7d88	ekf2: let controlAirDataFusion() update yaw estimator TAS	2023-02-22 09:08:33 -05:00
Daniel Agar	60856ebe62	ekf2: move range buffer pop to controlRangeHeightFusion()	2023-02-22 09:08:33 -05:00
Daniel Agar	08f111f694	ekf2: consolidate airspeed fusion logic and helpers - pass new airspeed sample around when available - can't completely eliminate _airspeed_sample_delayed until resetWind() called from sideslip fusion is updated	2023-02-22 09:08:33 -05:00
Daniel Agar	241cee2bb7	ekf2: move airspeed buffer pop to controlAirDataFusion	2023-02-22 09:08:33 -05:00
Daniel Agar	0711a34d0e	ekf2: move flow buffer pop to controlOpticalFlowFusion	2023-02-22 09:08:33 -05:00
Daniel Agar	ed8cef6cf0	ekf2: move controlAuxVelFusion control.cpp -> auxvel_fusion.cpp	2023-02-22 09:08:33 -05:00
Daniel Agar	b4e845d7c0	ekf2: move controlDragFusion control.cpp -> drag_fusion.cpp	2023-02-22 09:08:33 -05:00
Daniel Agar	c08a387c5a	ekf2: move controlBetaFusion control.cpp -> sideslip_fusion.cpp	2023-02-22 09:08:33 -05:00
Daniel Agar	d61b8c21b2	ekf2: move controlAirDataFusion() control.cpp -> airspeed_fusion.cpp	2023-02-22 09:08:33 -05:00
Daniel Agar	d840f7f39f	ekf2: move controlGpsYawFusion() control.cpp -> gps_control.cpp	2023-02-22 09:08:33 -05:00
Hamish Willee	b66e15c4b9	generate_msg_docs.py - fix path to messages	2023-02-22 09:32:05 +01:00
Matthias Grob	f887ad6ebf	mc_att_control: allow commanding a yaw rate with zero throttle Still avoiding to build up absolute yaw error in that case.	2023-02-21 19:33:28 +01:00
Matthias Grob	2b0f7879bc	mc_att_control_main: separate yaw rate setpoint generation from absolute yaw reset	2023-02-21 19:33:28 +01:00
Matthias Grob	ce5cff55b7	mac_att_control: heading lock in stabilized only after ekf final yaw alignment	2023-02-21 19:33:28 +01:00
Silvan Fuhrer	f0571de731	MCAttitudeController: remove reset of yaw_sp when landed Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-21 19:33:28 +01:00
Beat Küng	fe3c1d0a92	logger: do not try to wait for mavlink ack when writing watchdog data As it might block for 2.5s (if mavlink is blocked) and therefore would not write to file before restoring the priorities.	2023-02-21 11:32:30 -05:00
Beat Küng	66b0f6eb35	log_writer_file: call fsync after reliable transfer ensures watchdog data is flushed immediately	2023-02-21 11:32:30 -05:00
Beat Küng	e4cef9f303	logger: update watchdog - reduce boost priority to PX4_WQ_HP_BASE - 6 - add cli command 'trigger_watchdog' to manually trigger watchdog - add perf counters when triggering watchdog - reduce top measurement to 300ms - restore priorities after 1.5s There are precautions in case the SD card code itself has a busy-loop.	2023-02-21 11:32:30 -05:00
Beat Küng	015ba62727	log_writer_file: do not call close() with mutex held Generally not an issue, but if close() takes long, or even busy-loops due to an underlying bug in the OS, it will block the main thread too.	2023-02-21 11:32:30 -05:00
Jaeyoung Lim	5676cc32bc	Optionally enable sensor simulations	2023-02-21 11:16:25 -05:00
Leonardo Garcia	3bdb42b6a7	mro/pixracerpro: add missing px4_platform_configure() call (#21158 )	2023-02-21 09:30:39 +01:00
MAD-CRAZY-MAN	3ab34fe5b1	ci: build thepeach FCC-K1 & FCC-R1	2023-02-20 21:56:08 -05:00
Silvan Fuhrer	94be17af8f	FWPositionControllre: only check acceptance radius to swich to loiter to reach WP alt Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 21:55:27 -05:00
Julian Oes	9ec6a4b1d7	icm42688p: fix comment about gyro and accel bits This really confused me. Signed-off-by: Julian Oes <julian@oes.ch>	2023-02-20 21:47:11 -05:00
Silvan Fuhrer	2008a447c3	FW PositionController: circular landing: publish orbit status Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 17:04:39 +01:00
Silvan Fuhrer	485785d81d	FW PositionController: circular landing: enable automatic landing aborts Enable automatic landing abort on timed out distance sensor reading also for the circular landing. Do not enable the no-terrain timeout check, as, opposed to the straight landing, we here don't know when to expect the distance sensor to get valid. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 17:04:39 +01:00
Silvan Fuhrer	37b6dccda9	Land: use MIS_LND_ABRT_ALT also in non-mission Land As we don't know the landing point altitude in non-mission landings, assume the worst case (abort right before touchdown) and thus always climb MIS_LND_ABRT_ALT on triggering an abort. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 17:04:39 +01:00
Silvan Fuhrer	c47210fc77	FWPositionController: add support for circular landings Add method for circular landing, that is used instead of the straight fixed-wing landings in case the landing is not part of a mission landing. Use straight landing if previous WP is valid, and the ciruclar otherwise. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 17:04:39 +01:00
Silvan Fuhrer	50d75c537e	FWPositionController: auto_landing(): move non-position handling to top Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 17:04:39 +01:00
Silvan Fuhrer	b94ed34406	FW Position Control: initializeAutoLanding(): pass only alt value of pos_sp_curr Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 17:04:39 +01:00
Silvan Fuhrer	805de8a6d9	Navigator: set position setpoint to current location instead of to NAN Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 17:04:39 +01:00
Silvan Fuhrer	eaa4180920	Navigator: remove ununsed argument from set_land_item() Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 17:04:39 +01:00
David Sidrane	1fb6b003fc	NuttX with backport ioexpander/gpio:Add gpio_pin_register_byname	2023-02-20 04:14:01 -08:00
Silvan Fuhrer	167e58abba	AirspeedSelector: remove unused variable Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 11:35:24 +01:00
Silvan Fuhrer	1acb07c600	Navigator: set _land_start_index to first item with a position after the marker _land_start_index is used to to start the mission from this item index, and to avoid to publish a triplet.current.type=IDLE, we need to fill it with the actual position setpoint that the vehicle should go to at the start of a mission landing. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-20 06:50:53 +01:00
bresch	fafcdbf4ed	tests: empty parentheses were disambiguated as a function declaration	2023-02-17 21:10:44 -05:00
bresch	8ebf47edb1	ekf2: stop mag fusion when there is no data anymore	2023-02-17 08:51:55 -05:00
Silvan Fuhrer	deabe9a38d	Navigator: accept yaw immedietaly if the flag heading_good_for_control is not set Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-17 13:34:30 +01:00
David Sidrane	d291207b9f	NuttX with mmcsd backports to prevent system hang on error	2023-02-17 10:16:15 +01:00
RomanBapst	b00efcd966	cleanup Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	7ef2bff0a2	FeasibilityChecker: Fixed bug and added unit test for it Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	2e50277695	improved function naming Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	8ecb550331	cleanup Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	00b1968a5c	more clang tidy stuff Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	b8d0a8821a	fixed clang tidy Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	c09263d53c	use correct type Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	11fd3ef71a	use legacy parameter system and cleaned up vehicle type Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	741fbb931d	fixed land requirement for VTOL Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	6e07af959f	fixed bug in Matrix library Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	925ad97ff3	added unit tests Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	11143def82	tried to add functional unit test Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
RomanBapst	ceb8f6e1d5	started with feasibility checks Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-02-16 11:28:41 +01:00
Christian Rauch	d6bb19e11b	drivers/linux_pwm_out: link mixer_module	2023-02-15 16:04:31 -05:00