NuttX disable priority inheritance and adjust parameter save scheduling

2026-06-20 16:10:34 +08:00 · 2023-02-13 22:52:25 -05:00
362 changed files with 5399 additions and 8375 deletions
@@ -110,8 +110,6 @@ 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,
@@ -170,7 +170,7 @@
            ]
        },
        {
-            "label": "gazebo",
+            "label": "ign gazebo",
            "type": "shell",
            "options": {
                "cwd": "${workspaceFolder}",
@@ -178,7 +178,7 @@
                    "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gz/models",
                }
            },
-            "command": "gz sim -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
+            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
            "isBackground": true,
            "presentation": {
                "echo": true,
@@ -191,7 +191,7 @@
                "close": false
            },
            "problemMatcher": [],
-            "dependsOn":["gazebo kill"]
+            "dependsOn":["ign gazebo kill"]
        },
        {
            "label": "gazebo-classic kill",
@@ -211,9 +211,9 @@
            "dependsOn":["px4_sitl_cleanup"]
        },
        {
-            "label": "gazebo kill",
+            "label": "ign gazebo kill",
            "type": "shell",
-            "command": "pkill -9 -f 'gz sim' || true",
+            "command": "pkill -9 -f 'ign gazebo' || true",
            "presentation": {
                "echo": true,
                "reveal": "never",
@@ -44,88 +44,81 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con

 ## Maintenance Team

-Note: This is the source of truth for the active maintainers of PX4 ecosystem.
+  * 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)

-| 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.
+See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github).

 ## Supported Hardware

-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/).
+This repository contains code supporting Pixhawk standard boards (best supported, best tested, recommended choice) and proprietary boards.

 ### 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)

-These boards fully comply with Pixhawk Standard, and are maintained by the PX4-Autopilot maintainers and Dronecode team
+### 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)

-* 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)
+Additional information about supported hardware can be found in [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/).

 ## Project Roadmap

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

 ## Project Governance
@@ -12,10 +12,6 @@
 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,11 +11,6 @@
 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,10 +11,6 @@
 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,10 +13,6 @@ 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,10 +13,6 @@ 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,11 +10,6 @@ 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,11 +11,6 @@ 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
@@ -1,12 +0,0 @@
-#!/bin/sh
-#
-# @name Gazebo x500 vision
-#
-# @type Quadrotor
-#
-
-. ${R}etc/init.d-posix/airframes/4001_gz_x500
-
-PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
-PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
-PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_vision}
@@ -1,6 +1,8 @@

 mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p

+# shellcheck disable=SC2154
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
 # shellcheck disable=SC2154
 mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
 mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
@@ -74,7 +74,6 @@ px4_add_romfs_files(
 	4002_gz_x500_depth
 	4003_gz_rc_cessna
 	4004_gz_standard_vtol
-	4005_gz_x500_vision

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -8,28 +8,11 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"

 	echo "INFO  [init] SIH simulator"

-	if [ -n "${PX4_HOME_LAT}" ]; then
-		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
-	fi
-
-	if [ -n "${PX4_HOME_LON}" ]; then
-		param set SIH_LOC_LON0 ${PX4_HOME_LON}
-	fi
-
 	if simulator_sih start; then

-		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
+		sensor_baro_sim start
+		sensor_gps_sim start
+		sensor_mag_sim start

 	else
 		echo "ERROR [init] simulator_sih failed to start"
@@ -94,22 +77,10 @@ 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
-			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
+			sensor_baro_sim start
+			sensor_gps_sim start
+			sensor_mag_sim start
+			sensor_airspeed_sim start

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -120,22 +91,10 @@ 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
-			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
+			sensor_baro_sim start
+			sensor_gps_sim start
+			sensor_mag_sim start
+			sensor_airspeed_sim start

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -147,22 +106,10 @@ 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
-			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
+			sensor_baro_sim start
+			sensor_gps_sim start
+			sensor_mag_sim start
+			sensor_airspeed_sim start

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -36,7 +36,6 @@ 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
@@ -122,12 +122,16 @@ param set-default VT_TRANS_MIN_TM 15
 param set-default VT_B_TRANS_DUR 8
 param set-default VT_FWD_THRUST_SC 4
 param set-default VT_F_TRANS_DUR 1
+param set-default VT_B_REV_OUT 0.5
 param set-default VT_B_TRANS_THR 0.7
 param set-default VT_TRANS_TIMEOUT 22
 param set-default VT_F_TRANS_RAMP 4

 param set-default COM_RC_OVERRIDE 0

+
+
+
 param set-default CA_AIRFRAME 2

 param set-default CA_ROTOR_COUNT 5
@@ -54,6 +54,14 @@ param set-default CBRK_AIRSPD_CHK 162128
 # Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
 param set-default GND_MAX_ANG 3.1415

+param set-default RBCLW_BAUD 8
+param set-default RBCLW_COUNTS_REV 1200
+param set-default RBCLW_ADDRESS 128
+# 104 corresponds to Telem 4
+param set-default RBCLW_SER_CFG 104
+# Start this driver after setting parameters, because the driver uses some of those parameters.
+# roboclaw start /dev/ttyS3
+
 # Set geometry & output configration
 param set-default CA_AIRFRAME 6
 param set-default CA_R_REV 3
@@ -17,6 +17,7 @@ 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")
+    msg_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"..")
    msg_files = get_msgs_list(msg_path)
    msg_files.sort()

@@ -164,8 +164,7 @@ 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('\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('\ttopic.{0} -= time_offset;'.format(field_name))

 	print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name))
 	print('\tbuf.offset += sizeof(topic.{:});'.format(field_name))
@@ -102,7 +102,7 @@ class ModuleDocumentation(object):
    def _handle_usage_param_int(self, args):
        assert(len(args) == 6) # option_char, default_val, min_val, max_val, description, is_optional
        option_char = self._get_option_char(args[0])
-        default_val = self._get_int(args[1])
+        default_val = int(args[1], 0)
        description = self._get_string(args[4])
        if self._is_bool_true(args[5]):
            self._usage_string += "     [-%s <val>]  %s\n" % (option_char, description)
@@ -214,9 +214,6 @@ class ModuleDocumentation(object):
            f = f[:-1]
        return float(f)

-    def _get_int(self, argument):
-        return int(eval(argument))
-
    def _is_string(self, argument):
        return len(argument) > 0 and argument[0] == '"'

@@ -310,8 +307,6 @@ class SourceParser(object):
            r'//.*?$|/\*.*?\*/|\'(?:\\.|[^\\\'])*\'|"(?:\\.|[^\\"])*"',
            re.DOTALL | re.MULTILINE)

-        self._define_pattern = re.compile(r'#define\s+(\w+?)[^\S\r\n]+(.+?)\s*?\n')
-
    def Parse(self, scope, contents):
        """
        Incrementally parse program contents and append all found documentations
@@ -321,9 +316,6 @@ class SourceParser(object):
        # remove comments from source
        contents = self._comment_remover(contents)

-        # replace preprocessor defines defined in file directly
-        contents = self._define_replacer(contents)
-
        extracted_function_calls = [] # list of tuples: (FUNC_NAME, list(ARGS))

        start_index = 0
@@ -387,15 +379,6 @@ class SourceParser(object):
                return s
        return re.sub(self._comment_remove_pattern, replacer, text)

-    def _define_replacer(self, text):
-        """ check for C preprocesor #define in text and replace with argument"""
-        text = re.sub(r"\\\s*?\n"," ",text)
-        define_iter = self._define_pattern.finditer(text)
-        for define_pattern in define_iter:
-            text = re.sub(r"\b" +re.escape(str(define_pattern.groups()[0])) + r"\b", re.escape(str(define_pattern.groups()[1])), text)
-        return text
-
-
    def _do_consistency_check(self, contents, scope, module_doc):
        """
        check the documentation for consistency with the code (arguments to
@@ -84,7 +84,6 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i
 	gdb \
 	git \
 	lcov \
-	libfuse2 \
 	libxml2-dev \
 	libxml2-utils \
 	make \
@@ -183,7 +182,6 @@ 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
@@ -219,34 +217,33 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	# Set Java 11 as default
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")

-	# Gazebo / Gazebo classic installation
+	# Install Gazebo
 	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
-
-		# 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
+		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+			ignition-fortress \
+			;
 	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,18 +84,6 @@
        <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>
@@ -209,6 +197,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name="left_elevon">
@@ -637,6 +626,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <joint name="RightWheelJoint" type="revolute">
@@ -653,6 +643,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <joint name="CenterWheelJoint" type="revolute">
@@ -669,6 +660,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
@@ -803,7 +795,7 @@
      <sub_topic>servo_3</sub_topic>
      <p_gain>10.0</p_gain>
    </plugin>
-    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_puller_joint</jointName>
      <linkName>rotor_puller</linkName>
      <turningDirection>cw</turningDirection>
@@ -134,18 +134,6 @@
        <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>
@@ -208,6 +196,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_1'>
@@ -271,6 +260,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_2'>
@@ -334,6 +324,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_3'>
@@ -397,6 +388,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

@@ -462,6 +454,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

@@ -215,18 +215,6 @@
          </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>
@@ -302,6 +290,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name="rotor_1">
@@ -374,6 +363,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name="rotor_2">
@@ -446,6 +436,7 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name="rotor_3">
@@ -518,9 +509,10 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
-   <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+   <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
      <jointName>rotor_0_joint</jointName>
      <linkName>rotor_0</linkName>
      <turningDirection>ccw</turningDirection>
@@ -1,11 +0,0 @@
-<?xml version="1.0"?>
-<model>
-  <name>x500-vision</name>
-  <version>1.0</version>
-  <sdf version="1.9">model.sdf</sdf>
-  <author>
-    <name>Jaeyoung Lim</name>
-    <email>jalim@ethz.ch</email>
-  </author>
-  <description>Model of the X500 with a odometry/external vision input.</description>
-</model>
@@ -1,13 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<sdf version='1.9'>
-  <model name='x500-vision'>
-    <include merge='true'>
-      <uri>x500</uri>
-    </include>
-      <plugin
-        filename="gz-sim-odometry-publisher-system"
-        name="gz::sim::systems::OdometryPublisher">
-        <dimensions>3</dimensions>
-      </plugin>
-  </model>
-</sdf>
@@ -5,22 +5,21 @@
      <real_time_factor>1.0</real_time_factor>
      <real_time_update_rate>250</real_time_update_rate>
    </physics>
-    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
-    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
-    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
-    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
-    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
-    <plugin name='gz::sim::systems::AirPressure' filename='gz-sim-air-pressure-system'/>
-    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
+    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
+    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
+    <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::Sensors' filename='ignition-gazebo-sensors-system'>
      <render_engine>ogre2</render_engine>
    </plugin>
    <gui fullscreen='false'>
      <plugin name='3D View' filename='GzScene3D'>
-        <gz-gui>
+        <ignition-gui>
          <title>3D View</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='string' key='state'>docked</property>
-        </gz-gui>
+        </ignition-gui>
        <engine>ogre2</engine>
        <scene>scene</scene>
        <ambient_light>0.5984631152222222 0.5984631152222222 0.5984631152222222</ambient_light>
@@ -28,7 +27,7 @@
        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
      </plugin>
      <plugin name='World control' filename='WorldControl'>
-        <gz-gui>
+        <ignition-gui>
          <title>World control</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
@@ -40,13 +39,13 @@
            <line own='left' target='left'/>
            <line own='bottom' target='bottom'/>
          </anchors>
-        </gz-gui>
+        </ignition-gui>
        <play_pause>1</play_pause>
        <step>1</step>
        <start_paused>1</start_paused>
      </plugin>
      <plugin name='World stats' filename='WorldStats'>
-        <gz-gui>
+        <ignition-gui>
          <title>World stats</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
@@ -58,7 +57,7 @@
            <line own='right' target='right'/>
            <line own='bottom' target='bottom'/>
          </anchors>
-        </gz-gui>
+        </ignition-gui>
        <sim_time>1</sim_time>
        <real_time>1</real_time>
        <real_time_factor>1</real_time_factor>
@@ -137,7 +137,6 @@ 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,16 +146,12 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +112,6 @@ 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 +125,7 @@ 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,7 +114,6 @@ 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
@@ -124,15 +123,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +114,6 @@ 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
@@ -124,15 +123,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +114,6 @@ 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
@@ -128,11 +127,7 @@ 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,7 +186,6 @@ 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
@@ -198,16 +197,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +144,6 @@ 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,17 +155,13 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +174,6 @@ 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
@@ -185,17 +184,13 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +135,6 @@ 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,15 +143,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +136,6 @@ 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
@@ -145,15 +144,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +116,6 @@ 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,15 +125,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +144,6 @@ 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,16 +155,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +144,6 @@ 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,16 +155,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +144,6 @@ 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,16 +155,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +145,6 @@ 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,16 +156,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +145,6 @@ 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,16 +156,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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
@@ -19,7 +19,6 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -50,6 +49,7 @@ CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -66,6 +66,7 @@ CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -4,27 +4,12 @@
 #------------------------------------------------------------------------------
 board_adc start

-# Variants
-# 1. Isolated {ICM42688p, ICM20948(with mag)}, body-fixed {ICM20649}
-# 2. Isolated {ICM42688p, ICM42688p}, body-fixed  {ICM20649, ICM45686, AK09918}
-# 3. Isolated {ICM42688p, ICM42688p}, body-fixed  {ICM45686, AK09918}
-
-# SPI4 is isolated, SPI1 is body-fixed
-
-# SPI4, isolated
+# SPI4
 ms5611 -s -b 4 start
+icm42688p -s -b 4 -R 10 start
+icm20948 -s -b 4 -R 10 -M start

-icm42688p -s -b 4 -R 10 start -c 15
-if ! icm20948 -s -b 4 -R 10 -M -q start
-then
-	icm42688p -s -b 4 -R 6 start -c 13
-fi
-
-# SPI1, body-fixed
-if ! icm45686 -s -b 1 -R 3 -q start
-then
-	icm20649 -s -b 1 start
-fi
-
+# SPI1
 ms5611 -s -b 1 start
+icm20649 -s -b 1 start

@@ -145,7 +145,6 @@ 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,16 +156,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +145,6 @@ 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,16 +156,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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
@@ -44,16 +44,6 @@
 #include <stdint.h>
 #include <stm32_gpio.h>

-/**
- * If NuttX is built without support for SMPS it can brick the hardware.
- * Therefore, we make sure the NuttX headers are correct.
- */
-#include "hardware/stm32h7x3xx_pwr.h"
-#if STM32_PWR_CR3_SMPSEXTHP != (1 << 3)
-#  error "No SMPS support in NuttX submodule");
-#endif
-
-
 /* PX4IO connection configuration */
 #define BOARD_USES_PX4IO_VERSION       2
 #define PX4IO_SERIAL_DEVICE            "/dev/ttyS3"
@@ -38,7 +38,6 @@
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), // MPU_CS, MPU_DRDY
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortG, GPIO::Pin1}), // ICM45686_CS
 		initSPIDevice(DRV_BARO_DEVTYPE_MS5611,  SPI::CS{GPIO::PortD, GPIO::Pin7}), // BARO_CS
 	}),

@@ -49,7 +48,6 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI4, {
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin4}),  // MPU_EXT_CS
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
 		initSPIDevice(DRV_BARO_DEVTYPE_MS5611, 	SPI::CS{GPIO::PortC, GPIO::Pin14}), // BARO_EXT_CS
 	}),
 };
@@ -144,7 +144,6 @@ 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,17 +155,13 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +102,6 @@ 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
@@ -110,15 +109,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +135,6 @@ 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,15 +143,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +129,6 @@ 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
@@ -141,15 +140,11 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +117,6 @@ 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,15 +126,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +146,6 @@ 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
@@ -158,16 +157,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +147,6 @@ 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,15 +156,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +146,6 @@ 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
@@ -158,15 +157,11 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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
@@ -22,10 +22,13 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050

-# use EKF2 without mag
+# use the Q attitude estimator, it works w/o mag or GPS.
+param set-default SYS_MC_EST_GROUP 3
+param set-default ATT_ACC_COMP 0
+param set-default ATT_W_ACC 0.4000
+param set-default ATT_W_GYRO_BIAS 0.0000
+
 param set-default SYS_HAS_MAG 0
-# and enable gravity fusion
-param set-default EKF2_IMU_CONTROL 7

 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -38,5 +41,11 @@ param set-default SYS_DM_BACKEND 1
 # Ignore that there is no SD card
 param set-default COM_ARM_SDCARD 0

+# Store missions in RAM
+param set-default SYS_DM_BACKEND 1
+
+# Ignore that there is no SD card
+param set-default COM_ARM_SDCARD 0
+
 # Don't try to log onto SD card
 param set-default SDLOG_MODE -1
@@ -145,7 +145,6 @@ 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,15 +156,11 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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
@@ -22,10 +22,13 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050

-# use EKF2 without mag
+# use the Q attitude estimator, it works w/o mag or GPS.
+param set-default SYS_MC_EST_GROUP 3
+param set-default ATT_ACC_COMP 0
+param set-default ATT_W_ACC 0.4000
+param set-default ATT_W_GYRO_BIAS 0.0000
+
 param set-default SYS_HAS_MAG 0
-# and enable gravity fusion
-param set-default EKF2_IMU_CONTROL 7

 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -145,7 +145,6 @@ 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,15 +156,11 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +144,6 @@ 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,17 +155,13 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +135,6 @@ 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
@@ -145,15 +144,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +139,6 @@ 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,16 +147,12 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +141,6 @@ 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
@@ -152,16 +151,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +140,6 @@ 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
@@ -149,16 +148,12 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +143,6 @@ 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,16 +154,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +149,6 @@ 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
@@ -161,16 +160,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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
@@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_MUORB_SLPI=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_COMMANDER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
@@ -146,7 +146,6 @@ 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
@@ -158,16 +157,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +143,6 @@ 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,17 +154,13 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +143,6 @@ 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,17 +154,13 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +144,6 @@ 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,16 +155,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +144,6 @@ 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,16 +155,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +145,6 @@ 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,16 +156,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +195,5 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	sdio_mediachange(sdio_dev, true);
 #endif /* CONFIG_MMCSD */

-	px4_platform_configure();
-
 	return OK;
 }
@@ -143,7 +143,6 @@ 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,17 +154,13 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +138,6 @@ 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,16 +147,12 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +166,6 @@ 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
@@ -175,14 +174,13 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +169,6 @@ 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
@@ -178,14 +177,13 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +169,6 @@ 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
@@ -178,14 +177,13 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +168,6 @@ 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,14 +176,13 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +169,6 @@ 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
@@ -178,14 +177,13 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +182,6 @@ 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
@@ -197,12 +196,8 @@ 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,8 +4,6 @@ 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,7 +15,6 @@ 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
@@ -23,11 +22,4 @@ then
 	ifup can1
 	ifup can2
 	ifup can3
-fi
-if param greater -s CYPHAL_ENABLE 0
-then
-	ifup can0
-	ifup can1
-	ifup can2
-	ifup can3
-fi
+fi
@@ -203,7 +203,6 @@ 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 +253,7 @@ CONFIG_S32K3XX_QSPI=y
 CONFIG_S32K3XX_WKPUINTS=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
@@ -262,7 +261,6 @@ 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,7 +204,6 @@ 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
@@ -255,7 +254,7 @@ CONFIG_S32K3XX_QSPI=y
 CONFIG_S32K3XX_WKPUINTS=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_FILTER=y
 CONFIG_SCHED_INSTRUMENTATION_FILTER_DEFAULT_MODE=0x0B
@@ -269,7 +268,6 @@ 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,13 +116,10 @@ CONFIG_S32K1XX_PORTEINTS=y
 CONFIG_S32K1XX_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +97,6 @@ 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
@@ -105,15 +104,11 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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=n
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
@@ -132,7 +132,6 @@ 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
@@ -143,16 +142,12 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +138,6 @@ 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,16 +147,12 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +137,6 @@ 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,16 +146,12 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +138,6 @@ 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,16 +147,12 @@ CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +137,6 @@ 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,16 +148,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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,7 +138,6 @@ 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
@@ -150,16 +149,12 @@ CONFIG_READLINE_TABCOMPLETION=y
 CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
-CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_HPWORKSTACKSIZE=1632
 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
--- a/Show More
+++ b/Show More