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-19 08:50:34 +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
251 changed files with 5764 additions and 2642 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,11 @@ 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
 param set-default EKF2_MAG_YAWLIM 0

@@ -11,6 +11,11 @@ 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
 # VTOL no longer reports 0A for all ESCs in SITL
 param set-default FD_ACT_EN 0
@@ -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,10 +86,22 @@ 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
-			sensor_airspeed_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
+			fi

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -91,10 +112,22 @@ 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
-			sensor_airspeed_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
+			fi

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -106,10 +139,22 @@ 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
-			sensor_airspeed_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
+			fi

 		else
 			echo "ERROR [init] gz_bridge failed to 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>
@@ -137,6 +137,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -146,12 +147,16 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -112,6 +112,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -125,7 +126,11 @@ CONFIG_SCHED_HPWORKSTACKSIZE=3000
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -114,6 +114,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -123,11 +124,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -114,6 +114,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -123,11 +124,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -114,6 +114,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -127,7 +128,11 @@ CONFIG_SCHED_HPWORKSTACKSIZE=3000
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -186,6 +186,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -197,12 +198,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,13 +156,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -174,6 +174,7 @@ CONFIG_NSH_TELNET_LOGIN=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=245760
@@ -184,13 +185,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -135,6 +135,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=196608
@@ -143,11 +144,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -136,6 +136,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=196608
@@ -144,11 +145,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -116,6 +116,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -125,11 +126,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,12 +156,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,12 +156,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,12 +156,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,12 +157,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,12 +157,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -3,9 +3,9 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_UAVCAN=n
-CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,12 +157,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,12 +157,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,13 +156,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -102,6 +102,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=196608
 CONFIG_RAM_START=0x20000000
@@ -109,11 +110,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -135,6 +135,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=196608
@@ -143,11 +144,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -129,6 +129,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -140,11 +141,15 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -117,6 +117,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -126,11 +127,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -146,6 +146,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -157,12 +158,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -147,6 +147,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,11 +157,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -146,6 +146,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -157,11 +158,15 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,11 +157,15 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,11 +157,15 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,13 +156,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -135,6 +135,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -144,11 +145,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -139,6 +139,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=245760
@@ -147,12 +148,16 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -141,6 +141,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=245760
@@ -151,12 +152,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -140,6 +140,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=245760
@@ -148,12 +149,16 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -143,6 +143,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -154,12 +155,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -149,6 +149,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -160,12 +161,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -146,6 +146,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -157,12 +158,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -143,6 +143,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -154,13 +155,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -143,6 +143,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -154,13 +155,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,12 +156,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,12 +156,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,12 +157,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -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;
 }
@@ -143,6 +143,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -154,13 +155,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -138,6 +138,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -147,12 +148,16 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -166,6 +166,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
@@ -174,13 +175,14 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -169,6 +169,7 @@ CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=1024
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
@@ -177,13 +178,14 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -169,6 +169,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
@@ -177,13 +178,14 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -168,6 +168,7 @@ CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=1024
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
@@ -176,13 +177,14 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -169,6 +169,7 @@ CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=1024
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
@@ -177,13 +178,14 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -182,6 +182,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=1048576
@@ -196,8 +197,12 @@ CONFIG_SCHED_HPWORKSTACKSIZE=1800
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -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
@@ -203,6 +203,7 @@ CONFIG_NSH_TELNET_LOGIN=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=272000
@@ -253,7 +254,7 @@ CONFIG_S32K3XX_QSPI=y
 CONFIG_S32K3XX_WKPUINTS=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
@@ -261,6 +262,7 @@ CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1768
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -204,6 +204,7 @@ CONFIG_NSH_TELNET_LOGIN=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=272000
@@ -254,7 +255,7 @@ CONFIG_S32K3XX_QSPI=y
 CONFIG_S32K3XX_WKPUINTS=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_FILTER=y
 CONFIG_SCHED_INSTRUMENTATION_FILTER_DEFAULT_MODE=0x0B
@@ -268,6 +269,7 @@ CONFIG_SEGGER_RTT_MAX_NUM_UP_BUFFERS=12
 CONFIG_SEGGER_SYSVIEW=y
 CONFIG_SEGGER_SYSVIEW_PREFIX=y
 CONFIG_SEGGER_SYSVIEW_RTT_BUFFER_SIZE=4096
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -116,10 +116,13 @@ CONFIG_S32K1XX_PORTEINTS=y
 CONFIG_S32K1XX_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
@@ -97,6 +97,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=196608
 CONFIG_RAM_START=0x20000000
@@ -104,11 +105,15 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -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
@@ -132,6 +132,7 @@ CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_DEFAULT_PRIO_INHERIT=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
@@ -142,12 +143,16 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -138,6 +138,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -147,12 +148,16 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -137,6 +137,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -146,12 +147,16 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -138,6 +138,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -147,12 +148,16 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -137,6 +137,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -148,12 +149,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -138,6 +138,7 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -149,12 +150,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=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
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,13 +157,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -145,6 +145,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -156,13 +157,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -189,6 +189,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -210,6 +211,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1728
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -143,6 +143,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -154,13 +155,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -149,6 +149,7 @@ CONFIG_NUTTX_USERSPACE=0x08100000
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -160,13 +161,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -144,6 +144,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -155,13 +156,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -143,6 +143,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -154,13 +155,17 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -182,6 +182,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -193,12 +194,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -180,6 +180,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -191,12 +192,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -148,6 +148,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -159,11 +160,15 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -148,6 +148,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -159,12 +160,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=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)
@@ -186,6 +186,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -197,12 +198,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -71,6 +71,7 @@ CONFIG_NSH_USBCONSOLE=y
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=270336
 CONFIG_RAM_START=0x20000000
@@ -83,11 +84,15 @@ CONFIG_RP2040_SPI=y
 CONFIG_RP2040_UART1=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -178,6 +178,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
@@ -189,12 +190,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
@@ -138,6 +138,7 @@ CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=294912
@@ -148,12 +149,16 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1632
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
+CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
--- a/Show More
+++ b/Show More