lib/sensor_calibration: FindCurrentCalibrationIndex only call param_find once

2026-06-03 01:40:04 +08:00 · 2023-02-14 19:28:08 -05:00
272 changed files with 5242 additions and 7803 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,9 +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
@@ -11,9 +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
@@ -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,18 +77,9 @@ 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
+			sensor_baro_sim start
+			sensor_gps_sim start
+			sensor_mag_sim start
 			if param compare -s SENS_EN_ARSPDSIM 1
 			then
 				sensor_airspeed_sim start
@@ -120,18 +94,9 @@ 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
+			sensor_baro_sim start
+			sensor_gps_sim start
+			sensor_mag_sim start
 			if param compare -s SENS_EN_ARSPDSIM 1
 			then
 				sensor_airspeed_sim start
@@ -147,18 +112,9 @@ 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
+			sensor_baro_sim start
+			sensor_gps_sim start
+			sensor_mag_sim start
 			if param compare -s SENS_EN_ARSPDSIM 1
 			then
 				sensor_airspeed_sim 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>
@@ -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

@@ -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
 	}),
 };
@@ -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
@@ -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
@@ -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
@@ -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;
 }
@@ -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
@@ -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
@@ -4,4 +4,3 @@ CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
 CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
 CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_ESC_CONTROLLER=y
@@ -47,7 +47,6 @@ then
 	fi
 fi

-
 if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004
 then
 	# Internal SPI bus ICM20649
@@ -102,13 +101,7 @@ if ver hwtypecmp V6X002001
 then
 	rm3100 -I -b 4 start
 else
-	if ver hwtypecmp V6X009010 V6X010010
-	then
-		# Internal magnetometer on I2C
-		bmm150 -I -R 0 start
-	else
-		bmm150 -I -R 6 start
-	fi
+	bmm150 -I -R 6 start
 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)
@@ -5,8 +5,13 @@ uint8 INDEX_ROLL = 0
 uint8 INDEX_PITCH = 1
 uint8 INDEX_YAW = 2
 uint8 INDEX_THROTTLE = 3
+uint8 INDEX_FLAPS = 4
+uint8 INDEX_SPOILERS = 5
+uint8 INDEX_AIRBRAKES = 6
+uint8 INDEX_LANDING_GEAR = 7
 uint8 INDEX_GIMBAL_SHUTTER = 3
 uint8 INDEX_CAMERA_ZOOM = 4
+uint8 INDEX_COLLECTIVE_TILT = 8

 uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
@@ -5,4 +5,4 @@ uint32 noutputs				# valid outputs
 float32[16] output				# output data, in natural output units

 # actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1])
-# TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug
+# TOPICS actuator_outputs actuator_outputs_sim
@@ -128,7 +128,6 @@ set(msg_files
 	MountOrientation.msg
 	ModeCompleted.msg
 	NavigatorMissionItem.msg
-	NormalizedUnsignedSetpoint.msg
 	NpfgStatus.msg
 	ObstacleDistance.msg
 	OffboardControlMode.msg
@@ -179,7 +178,6 @@ set(msg_files
 	TaskStackInfo.msg
 	TecsStatus.msg
 	TelemetryStatus.msg
-	TiltrotorExtraControls.msg
 	TimesyncStatus.msg
 	TrajectoryBezier.msg
 	TrajectorySetpoint.msg
@@ -220,6 +218,7 @@ set(msg_files
 	VehicleTrajectoryBezier.msg
 	VehicleTrajectoryWaypoint.msg
 	VtolVehicleStatus.msg
+	WheelEncoders.msg
 	Wind.msg
 	YawEstimatorStatus.msg
 )
@@ -5,14 +5,12 @@ float32 esc_voltage					# Voltage measured from current ESC [V] - if supported
 float32 esc_current					# Current measured from current ESC [A] - if supported
 float32 esc_temperature					# Temperature measured from current ESC [degC] - if supported
 uint8 esc_address					# Address of current ESC (in most cases 1-8 / must be set by driver)
-uint8 esc_cmdcount					# Counter of number of commands

 uint8 esc_state					# State of ESC - depend on Vendor

 uint8 actuator_function				# actuator output function (one of Motor1...MotorN)

 uint16 failures					# Bitmask to indicate the internal ESC faults
-int8 esc_power					# Applied power 0-100 in % (negative values reserved)

 uint8 FAILURE_OVER_CURRENT = 0 			# (1 << 0)
 uint8 FAILURE_OVER_VOLTAGE = 1 			# (1 << 1)
@@ -17,7 +17,6 @@ uint32 mode_req_offboard_signal
 uint32 mode_req_home_position
 uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
 uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
-uint32 mode_req_manual_control
 uint32 mode_req_other             # other requirements, not covered above (for external modes)


@@ -29,6 +28,7 @@ bool local_position_invalid           # Local position estimate invalid
 bool local_position_invalid_relaxed   # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
 bool local_velocity_invalid           # Local velocity estimate invalid
 bool global_position_invalid          # Global position estimate invalid
+bool gps_position_invalid             # GPS position invalid
 bool auto_mission_missing             # No mission available
 bool offboard_control_signal_lost     # Offboard signal lost
 bool home_position_invalid            # No home position available
@@ -26,7 +26,7 @@ float32 pitch    # move forward, negative pitch rotation, nose down
 float32 yaw      #               positive yaw rotation,   clockwise when seen top down
 float32 throttle # move up,      positive thrust,         -1 is minimum available 0% or -100% +1 is 100% thrust

-float32 flaps			 # position of flaps switch/knob/lever [-1, 1]
+float32 flaps			 # flap position

 float32 aux1
 float32 aux2
@@ -1,5 +0,0 @@
-uint64 timestamp        		# time since system start (microseconds)
-
-float32 normalized_setpoint          	# [0, 1]
-
-# TOPICS flaps_setpoint spoilers_setpoint
@@ -28,3 +28,8 @@ float32 throttle_trim			# estimated throttle value [0,1] required to fly level a
 uint8 mode
 uint8 TECS_MODE_NORMAL = 0
 uint8 TECS_MODE_UNDERSPEED = 1
+uint8 TECS_MODE_TAKEOFF = 2
+uint8 TECS_MODE_LAND = 3
+uint8 TECS_MODE_LAND_THROTTLELIM = 4
+uint8 TECS_MODE_BAD_DESCENT = 5
+uint8 TECS_MODE_CLIMBOUT = 6
@@ -1,4 +0,0 @@
-uint64 timestamp # time since system start (microseconds)
-
-float32 collective_tilt_normalized_setpoint	# Collective tilt angle of motors of tiltrotor, 0: vertical, 1: horizontal [0, 1]
-float32 collective_thrust_normalized_setpoint 	# Collective thrust setpoint [0, 1]
@@ -17,4 +17,14 @@ bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)

 bool fw_control_yaw_wheel	# control heading with steering wheel (used for auto takeoff on runway)

+uint8 apply_flaps       	# flap config specifier
+uint8 FLAPS_OFF = 0     	# no flaps
+uint8 FLAPS_LAND = 1    	# landing config flaps
+uint8 FLAPS_TAKEOFF = 2 	# take-off config flaps
+
+uint8 apply_spoilers		# spoiler config specifier
+uint8 SPOILERS_OFF = 0     	# no spoilers
+uint8 SPOILERS_LAND = 1    	# landing config spoiler
+uint8 SPOILERS_DESCEND = 2 	# descend config spoiler
+
 # TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
@@ -0,0 +1,5 @@
+uint64 timestamp			# time since system start (microseconds)
+
+int64 encoder_position   # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation
+int32 speed              # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse
+uint32 pulses_per_rev    # Number of pulses per revolution for each wheel
@@ -99,10 +99,7 @@ __END_DECLS

 #define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR

-// Qurt doesn't have an SD card for storage
-#ifndef __PX4_QURT
 #define PX4_STORAGEDIR PX4_ROOTFSDIR
-#endif

 /****************************************************************************
 * Defines for POSIX and ROS
@@ -76,18 +76,18 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};

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

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

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

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

-// Logger watchdog priority, triggered when a task busy-loops (and
-// restored after a short time).
-// The priority is a trade-off between:
-// - ability to capture any busy-looping task below this priority
-// - not having a negative impact on the system itself
-#define SCHED_PRIORITY_LOG_WATCHDOG		(PX4_WQ_HP_BASE - 6)
-
 // Position controllers typically are in a blocking wait on estimator data
 // so when new sensor data is available they will run last. Keeping them
 // on a high priority ensures that they are the first process to be run
@@ -347,12 +347,12 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool
 					data_offset += sizeof(uint64_t);

 				} else if (strcmp(c_type, "float") == 0) {
-					if (!dont_print) { PX4_INFO_RAW("%.5f", (double) * (float *)(data_ptr + data_offset)); }
+					if (!dont_print) { PX4_INFO_RAW("%.4f", (double) * (float *)(data_ptr + data_offset)); }

 					data_offset += sizeof(float);

 				} else if (strcmp(c_type, "double") == 0) {
-					if (!dont_print) { PX4_INFO_RAW("%.6f", *(double *)(data_ptr + data_offset)); }
+					if (!dont_print) { PX4_INFO_RAW("%.4f", *(double *)(data_ptr + data_offset)); }

 					data_offset += sizeof(double);

@@ -14,11 +14,11 @@
            "environment": [
                {
                    "name": "PX4_SIM_MODEL",
-                    "value": "gz_${input:PX4_GZ_MODEL}"
+                    "value": "${input:PX4_GZ_MODEL}"
                }
            ],
            "externalConsole": false,
-            "postDebugTask": "gazebo kill",
+            "postDebugTask": "ign gazebo kill",
            "linux": {
                "MIMode": "gdb",
                "externalConsole": false,
@@ -222,9 +222,6 @@
            "description": "GZ vehicle model",
            "options": [
              "x500",
-              "x500_depth",
-              "rc_cessna",
-              "standard_vtol",
            ],
            "default": "x500"
        }
@@ -105,8 +105,7 @@ static int create_dirs();
 static int run_startup_script(const std::string &commands_file, const std::string &absolute_binary_path, int instance);
 static std::string get_absolute_binary_path(const std::string &argv0);
 static void wait_to_exit();
-static int get_server_running(int instance, bool *is_running);
-static int set_server_running(int instance);
+static bool is_server_running(int instance, bool server);
 static void print_usage();
 static bool dir_exists(const std::string &path);
 static bool file_exists(const std::string &name);
@@ -125,7 +124,6 @@ int main(int argc, char **argv)
 {
 	bool is_client = false;
 	bool pxh_off = false;
-	bool server_is_running = false;

 	/* Symlinks point to all commands that can be used as a client with a prefix. */
 	const char prefix[] = PX4_SHELL_COMMAND_PREFIX;
@@ -133,9 +131,6 @@ int main(int argc, char **argv)

 	std::string absolute_binary_path; // full path to the px4 binary being executed

-	int ret = PX4_OK;
-	int instance = 0;
-
 	if (argc > 0) {
 		/* The executed binary name could start with a path, so strip it away */
 		const std::string full_binary_name = argv[0];
@@ -151,6 +146,8 @@ int main(int argc, char **argv)
 	}

 	if (is_client) {
+		int instance = 0;
+
 		if (argc >= 3 && strcmp(argv[1], "--instance") == 0) {
 			instance = strtoul(argv[2], nullptr, 10);
 			/* update argv so that "--instance <instance>" is not visible anymore */
@@ -163,16 +160,15 @@ int main(int argc, char **argv)

 		PX4_DEBUG("instance: %i", instance);

-		ret = get_server_running(instance, &server_is_running);
+		if (!is_server_running(instance, false)) {
+			if (errno) {
+				PX4_ERR("Failed to communicate with daemon: %s", strerror(errno));

-		if (ret != PX4_OK) {
-			PX4_ERR("PX4 client failed to get server status");
-			return ret;
-		}
+			} else {
+				PX4_ERR("PX4 daemon not running yet");
+			}

-		if (!server_is_running) {
-			PX4_ERR("PX4 server not running");
-			return PX4_ERROR;
+			return -1;
 		}

 		/* Remove the path and prefix. */
@@ -206,6 +202,7 @@ int main(int argc, char **argv)

 		bool working_directory_default = false;

+		int instance = 0;
 		bool instance_provided = false;

 		int myoptind = 1;
@@ -295,27 +292,20 @@ int main(int argc, char **argv)
 				PX4_INFO("working directory %s", working_directory.c_str());
 			}

-			ret = change_directory(working_directory);
+			int ret = change_directory(working_directory);

 			if (ret != PX4_OK) {
 				return ret;
 			}
 		}

-		ret = get_server_running(instance, &server_is_running);
-
-		if (ret != PX4_OK) {
-			PX4_ERR("Failed to get server status");
-			return ret;
-		}
-
-		if (server_is_running) {
+		if (is_server_running(instance, true)) {
 			// allow running multiple instances, but the server is only started for the first
-			PX4_INFO("PX4 server already running for instance %i", instance);
-			return PX4_ERROR;
+			PX4_INFO("PX4 daemon already running for instance %i (%s)", instance, strerror(errno));
+			return -1;
 		}

-		ret = create_symlinks_if_needed(data_path);
+		int ret = create_symlinks_if_needed(data_path);

 		if (ret != PX4_OK) {
 			return ret;
@@ -353,13 +343,6 @@ int main(int argc, char **argv)
 		px4::init_once();
 		px4::init(argc, argv, "px4");

-		// Don't set this up until PX4 is up and running
-		ret = set_server_running(instance);
-
-		if (ret != PX4_OK) {
-			return ret;
-		}
-
 		ret = run_startup_script(commands_file, absolute_binary_path, instance);

 		if (ret == 0) {
@@ -635,73 +618,39 @@ void print_usage()
 	printf("        e.g.: px4-commander status\n");
 }

-int get_server_running(int instance, bool *is_server_running)
+bool is_server_running(int instance, bool server)
 {
 	const std::string file_lock_path = std::string(LOCK_FILE_PATH) + '-' + std::to_string(instance);
 	int fd = open(file_lock_path.c_str(), O_RDWR | O_CREAT, 0666);

 	if (fd < 0) {
-		PX4_ERR("%s: failed to create lock file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
-		return PX4_ERROR;
+		PX4_ERR("is_server_running: failed to create lock file: %s, reason=%s", file_lock_path.c_str(), strerror(errno));
+		return false;
 	}

-	int status = PX4_OK;
-	struct flock lock;
-	memset(&lock, 0, sizeof(struct flock));
+	bool result = false;

-	// Exclusive write lock, cover the entire file (regardless of size)
-	lock.l_type = F_WRLCK;
-	lock.l_whence = SEEK_SET;
-
-	if (fcntl(fd, F_GETLK, &lock) < 0) {
-		PX4_ERR("%s: failed to get check for lock on file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
-		status = PX4_ERROR;
-
-	} else {
-		// F_GETLK will set l_type to F_UNLCK if no one had a lock on the file. Otherwise,
-		// it means that the server is running and has a lock on the file
-		if (lock.l_type != F_UNLCK) {
-			*is_server_running = true;
+	// Server is running if the file is already locked.
+	if (flock(fd, LOCK_EX | LOCK_NB) < 0) {
+		if (errno == EWOULDBLOCK) {
+			// a server is running!
+			result = true;

 		} else {
-			*is_server_running = false;
+			PX4_ERR("is_server_running: failed to get lock on file: %s, reason=%s", file_lock_path.c_str(), strerror(errno));
+			result = false;
 		}
 	}

-	close(fd);
-
-	return status;
-}
-
-int set_server_running(int instance)
-{
-	const std::string file_lock_path = std::string(LOCK_FILE_PATH) + '-' + std::to_string(instance);
-	int fd = open(file_lock_path.c_str(), O_RDWR | O_CREAT, 0666);
-
-	if (fd < 0) {
-		PX4_ERR("%s: failed to create lock file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
-		return PX4_ERROR;
-	}
-
-	int status = PX4_OK;
-
-	struct flock lock;
-	memset(&lock, 0, sizeof(struct flock));
-
-	// Exclusive lock, cover the entire file (regardless of size).
-	lock.l_type = F_WRLCK;
-	lock.l_whence = SEEK_SET;
-
-	if (fcntl(fd, F_SETLK, &lock) < 0) {
-		PX4_ERR("%s: failed to set lock on file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
-		status = PX4_ERROR;
+	if (result || !server) {
 		close(fd);
 	}

-	// note: server leaks the file handle, on purpose, in order to keep the lock on the file until the process terminates.
+	// note: server leaks the file handle once, on purpose, in order to keep the lock on the file until the process terminates.
 	// In this case we return false so the server code path continues now that we have the lock.

-	return status;
+	errno = 0;
+	return result;
 }

 bool file_exists(const std::string &name)
@@ -166,12 +166,11 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma
 				return -1;

 			} else {
-				strcpy(taskmap[task_index].argv_storage[i], argv[i]);
+				//px4_clock_gettimemap[task_index].argv_storage[i], argv[i]);
 				taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i];
 			}

 		} else {
-			// Must add NULL at end of argv
 			taskmap[task_index].argv[i] = nullptr;
 			break;
 		}
@@ -421,13 +420,13 @@ int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
 	return 0;
 }

-int px4_prctl(int option, const char *arg2, px4_task_t pid)
+int px4_prctl(int option, const char *arg2, pthread_t pid)
 {
 	int rv = -1;
 	pthread_mutex_lock(&task_mutex);

 	for (int i = 0; i < PX4_MAX_TASKS; i++) {
-		if (taskmap[i].isused && taskmap[i].tid == (pthread_t) pid) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
 			rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2);
 			return rv;
 		}
@@ -65,13 +65,12 @@ ModalIo::ModalIo() :
 		_esc_status.esc[i].esc_address     = 0;
 		_esc_status.esc[i].esc_rpm         = 0;
 		_esc_status.esc[i].esc_state       = 0;
-		_esc_status.esc[i].esc_cmdcount    = 0;
+		//_esc_status.esc[i].esc_cmdcount    = 0;
 		_esc_status.esc[i].esc_voltage     = 0;
 		_esc_status.esc[i].esc_current     = 0;
 		_esc_status.esc[i].esc_temperature = 0;
 		_esc_status.esc[i].esc_errorcount  = 0;
 		_esc_status.esc[i].failures        = 0;
-		_esc_status.esc[i].esc_power       = 0;
 	}

 	qc_esc_packet_init(&_fb_packet);
@@ -153,8 +152,6 @@ int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map)
 	param_get(param_find("MODAL_IO_RPM_MIN"), &params->rpm_min);
 	param_get(param_find("MODAL_IO_RPM_MAX"), &params->rpm_max);

-	param_get(param_find("MODAL_IO_VLOG"),    &params->verbose_logging);
-
 	if (params->rpm_min >= params->rpm_max) {
 		PX4_ERR("Invalid parameter MODAL_IO_RPM_MIN.  Please verify parameters.");
 		params->rpm_min = 0;
@@ -339,9 +336,9 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
 					_esc_status.esc[id].esc_address  = motor_idx + 1; //remapped motor ID
 					_esc_status.esc[id].timestamp    = tnow;
 					_esc_status.esc[id].esc_rpm      = fb.rpm;
-					_esc_status.esc[id].esc_power    = fb.power;
+					//_esc_status.esc[id].esc_power    = fb.power;
 					_esc_status.esc[id].esc_state    = fb.id_state & 0x0F;
-					_esc_status.esc[id].esc_cmdcount = fb.cmd_counter;
+					//_esc_status.esc[id].esc_cmdcount = fb.cmd_counter;
 					_esc_status.esc[id].esc_voltage  = _esc_chans[id].voltage;
 					_esc_status.esc[id].esc_current  = _esc_chans[id].current;
 					_esc_status.esc[id].failures     = 0; //not implemented
@@ -588,7 +585,7 @@ int ModalIo::custom_command(int argc, char *argv[])
 	}

 	if (!strcmp(verb, "reset")) {
-		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
+		if (esc_id < 4) {
 			PX4_INFO("Reset ESC: %i", esc_id);
 			cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = false;
@@ -600,7 +597,7 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	} else if (!strcmp(verb, "version")) {
-		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
+		if (esc_id < 4) {
 			PX4_INFO("Request version for ESC: %i", esc_id);
 			cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = true;
@@ -613,7 +610,7 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	} else if (!strcmp(verb, "version-ext")) {
-		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
+		if (esc_id < 4) {
 			PX4_INFO("Request extended version for ESC: %i", esc_id);
 			cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = true;
@@ -626,14 +623,14 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	} else if (!strcmp(verb, "tone")) {
-		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
+		if (0 < esc_id && esc_id < 16) {
 			PX4_INFO("Request tone for ESC mask: %i", esc_id);
 			cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = false;
 			return get_instance()->send_cmd_thread_safe(&cmd);

 		} else {
-			print_usage("Invalid ESC ID, use 0-3");
+			print_usage("Invalid ESC mask, use 1-15");
 			return 0;
 		}

@@ -655,20 +652,42 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	}  else if (!strcmp(verb, "rpm")) {
-		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
-			PX4_INFO("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate);
-			int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
+		if (0 < esc_id && esc_id < 16) {
+			PX4_INFO("Request RPM for ESC bit mask: %i - RPM: %i", esc_id, rate);
+			int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS];
+			int16_t outputs[MODAL_IO_OUTPUT_CHANNELS];
+			outputs[0] = (esc_id & 1) ? rate : 0;
+			outputs[1] = (esc_id & 2) ? rate : 0;
+			outputs[2] = (esc_id & 4) ? rate : 0;
+			outputs[3] = (esc_id & 8) ? rate : 0;
+
+			//the motor mapping is.. if I want to spin Motor 1 (1-4) then i need to provide non-zero rpm for motor map[m-1]
+
+			modal_io_params_t params;
+			ch_assign_t map[MODAL_IO_OUTPUT_CHANNELS];
+			get_instance()->load_params(&params, (ch_assign_t *)&map);
+
+			uint8_t id_fb_raw = 0;
 			uint8_t id_fb = 0;

-			if (esc_id == 0xFF) {
-				rate_req[0] = rate;
-				rate_req[1] = rate;
-				rate_req[2] = rate;
-				rate_req[3] = rate;
+			if (esc_id & 1) { id_fb_raw = 0; }

-			} else {
-				rate_req[esc_id] = rate;
-				id_fb = esc_id;
+			else if (esc_id & 2) { id_fb_raw = 1; }
+
+			else if (esc_id & 4) { id_fb_raw = 2; }
+
+			else if (esc_id & 8) { id_fb_raw = 3; }
+
+			for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
+				int motor_idx = map[i].number - 1; // user defined mapping is 1-4, array is 0-3
+
+				if (motor_idx >= 0 && motor_idx < MODAL_IO_OUTPUT_CHANNELS) {
+					rate_req[i] = outputs[motor_idx] * map[i].direction;
+				}
+
+				if (motor_idx == id_fb_raw) {
+					id_fb = i;
+				}
 			}

 			cmd.len = qc_esc_create_rpm_packet4_fb(rate_req[0],
@@ -689,31 +708,53 @@ int ModalIo::custom_command(int argc, char *argv[])
 			cmd.repeat_delay_us = repeat_delay_us;
 			cmd.print_feedback  = true;

-			PX4_INFO("feedback id debug: %i", id_fb);
+			PX4_INFO("ESC map: %d %d %d %d", map[0].number, map[1].number, map[2].number, map[3].number);
+			PX4_INFO("feedback id debug: %i, %i", id_fb_raw, id_fb);
 			PX4_INFO("Sending UART ESC RPM command %i", rate);

 			return get_instance()->send_cmd_thread_safe(&cmd);

 		} else {
-			print_usage("Invalid ESC ID, use 0-3");
+			print_usage("Invalid ESC mask, use 1-15");
 			return 0;
 		}

 	} else if (!strcmp(verb, "pwm")) {
-		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
-			PX4_INFO("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate);
-			int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
+		if (0 < esc_id && esc_id < 16) {
+			PX4_INFO("Request PWM for ESC mask: %i - PWM: %i", esc_id, rate);
+			int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS];
+			int16_t outputs[MODAL_IO_OUTPUT_CHANNELS];
+			outputs[0] = (esc_id & 1) ? rate : 0;
+			outputs[1] = (esc_id & 2) ? rate : 0;
+			outputs[2] = (esc_id & 4) ? rate : 0;
+			outputs[3] = (esc_id & 8) ? rate : 0;
+
+			modal_io_params_t params;
+			ch_assign_t map[MODAL_IO_OUTPUT_CHANNELS];
+			get_instance()->load_params(&params, (ch_assign_t *)&map);
+
+			uint8_t id_fb_raw = 0;
 			uint8_t id_fb = 0;

-			if (esc_id == 0xFF) {
-				rate_req[0] = rate;
-				rate_req[1] = rate;
-				rate_req[2] = rate;
-				rate_req[3] = rate;
+			if (esc_id & 1) { id_fb_raw = 0; }

-			} else {
-				rate_req[esc_id] = rate;
-				id_fb = esc_id;
+			else if (esc_id & 2) { id_fb_raw = 1; }
+
+			else if (esc_id & 4) { id_fb_raw = 2; }
+
+			else if (esc_id & 8) { id_fb_raw = 3; }
+
+			for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
+				int motor_idx = map[i].number - 1; // user defined mapping is 1-4, array is 0-3
+
+				if (motor_idx >= 0 && motor_idx < MODAL_IO_OUTPUT_CHANNELS) {
+					rate_req[i] = outputs[motor_idx] * map[i].direction;
+					PX4_INFO("rate_req[%d]=%d", i, rate_req[i]);
+				}
+
+				if (motor_idx == id_fb_raw) {
+					id_fb = i;
+				}
 			}

 			cmd.len = qc_esc_create_pwm_packet4_fb(rate_req[0],
@@ -734,9 +775,11 @@ int ModalIo::custom_command(int argc, char *argv[])
 			cmd.repeat_delay_us = repeat_delay_us;
 			cmd.print_feedback  = true;

-			PX4_INFO("feedback id debug: %i", id_fb);
+			PX4_INFO("ESC map: %d %d %d %d", map[0].number, map[1].number, map[2].number, map[3].number);
+			PX4_INFO("feedback id debug: %i, %i", id_fb_raw, id_fb);
 			PX4_INFO("Sending UART ESC power command %i", rate);

+
 			return get_instance()->send_cmd_thread_safe(&cmd);

 		} else {
@@ -1114,7 +1157,8 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	//check_for_esc_timeout();

 	// publish the actual command that we sent and the feedback received
-	if (_parameters.verbose_logging) {
+	/*
+	if (MODALAI_PUBLISH_ESC_STATUS) {
 		actuator_outputs_s actuator_outputs{};
 		actuator_outputs.noutputs = num_outputs;

@@ -1125,10 +1169,9 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 		actuator_outputs.timestamp = hrt_absolute_time();

 		_outputs_debug_pub.publish(actuator_outputs);
-
+		_esc_status_pub.publish(_esc_status);
 	}
-
-	_esc_status_pub.publish(_esc_status);
+	*/

 	perf_count(_output_update_perf);

@@ -1320,9 +1363,7 @@ void ModalIo::Run()
 					}

 					if (_current_cmd.response) {
-						if (read_response(&_current_cmd) == 0) {
-							_esc_status_pub.publish(_esc_status);
-						}
+						read_response(&_current_cmd);
 					}

 				} else {
@@ -1392,19 +1433,19 @@ $ todo
 	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false);

 	PRINT_MODULE_USAGE_COMMAND_DESCR("rpm", "Closed-Loop RPM test control request");
-	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false);
 	PRINT_MODULE_USAGE_PARAM_INT('r', 0, -32768, 32768, "RPM, -32,768 to 32,768", false);
 	PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false);
 	PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false);

 	PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request");
-	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false);
 	PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false);
 	PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false);
 	PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false);

 	PRINT_MODULE_USAGE_COMMAND_DESCR("tone", "Send tone generation request to ESC");
-	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false);
 	PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 255, "Period of sound, inverse frequency, 0-255", false);
 	PRINT_MODULE_USAGE_PARAM_INT('d', 0, 0, 255, "Duration of the sound, 0-255, 1LSB = 13ms", false);
 	PRINT_MODULE_USAGE_PARAM_INT('v', 0, 0, 100, "Power (volume) of sound, 0-100", false);
@@ -143,7 +143,6 @@ private:
 		int32_t		function_map[MODAL_IO_OUTPUT_CHANNELS] {0, 0, 0, 0};
 		int32_t		motor_map[MODAL_IO_OUTPUT_CHANNELS] {1, 2, 3, 4};
 		int32_t		direction_map[MODAL_IO_OUTPUT_CHANNELS] {1, 1, 1, 1};
-		int32_t		verbose_logging{0};
 	} modal_io_params_t;

 	struct EscChan {
@@ -189,7 +188,7 @@ private:
 	uORB::Subscription 	_actuator_test_sub{ORB_ID(actuator_test)};
 	uORB::Subscription	_led_update_sub{ORB_ID(led_control)};

-	uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
+	//uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
 	uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};

 	modal_io_params_t	_parameters;
@@ -201,16 +201,3 @@ PARAM_DEFINE_INT32(MODAL_IO_T_EXPO, 35);
 * @increment 0.001
 */
 PARAM_DEFINE_FLOAT(MODAL_IO_T_COSP, 0.990);
-
-/**
- * UART ESC verbose logging
- *
- * @reboot_required true
- *
- * @group MODAL IO
- * @value 0 - Disabled
- * @value 1 - Enabled
- * @min 0
- * @max 1
- */
-PARAM_DEFINE_INT32(MODAL_IO_VLOG, 0);
@@ -53,7 +53,6 @@
 #include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/actuator_test.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
@@ -70,30 +69,61 @@ public:
 	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;

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

 	~UavcanEscController() {};

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

 			if (new_arming.armed != _armed.armed) {
 				_armed = new_arming;
-				publish_readiness();
-			}
-		}
+				size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;

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

@@ -101,7 +131,7 @@ public:
 	{
 		if (_port_id > 0) {
 			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
-			size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+			size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 	_pub_manager.printInfo();

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

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


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

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

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

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

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

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

 public:

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 #define DRV_GYR_DEVTYPE_MPU6050  0x35
 #define DRV_IMU_DEVTYPE_MPU6500  0x36
@@ -642,7 +642,7 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 	accel.timestamp_sample = timestamp_sample;
 	accel.samples = 0;

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

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

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

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

-		// shift by 1 (least significant bit is always 0)
 		gyro.x[gyro.samples] = gyro_x / 2;
 		gyro.y[gyro.samples] = gyro_y / 2;
 		gyro.z[gyro.samples] = gyro_z / 2;
@@ -1,752 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "ICM45686.hpp"
-
-using namespace time_literals;
-
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
-}
-
-static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
-}
-
-static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
-{
-	// 0xXXXAABBC
-	uint32_t high   = ((a << 12) & 0x000FF000);
-	uint32_t low    = ((b << 4)  & 0x00000FF0);
-	uint32_t lowest = (c         & 0x0000000F);
-
-	uint32_t x = high | low | lowest;
-
-	if (a & Bit7) {
-		// sign extend
-		x |= 0xFFF00000u;
-	}
-
-	return static_cast<int32_t>(x);
-}
-
-
-ICM45686::ICM45686(const I2CSPIDriverConfig &config) :
-	SPI(config),
-	I2CSPIDriver(config),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
-{
-	if (config.custom1 != 0) {
-		_enable_clock_input = true;
-		_input_clock_freq = config.custom1;
-		// TODO: this is not tested
-		ConfigureCLKIN();
-
-	} else {
-		_enable_clock_input = false;
-	}
-
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
-}
-
-ICM45686::~ICM45686()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_fifo_empty_perf);
-	perf_free(_fifo_overflow_perf);
-	perf_free(_fifo_reset_perf);
-}
-
-int ICM45686::init()
-{
-	int ret = SPI::init();
-
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
-		return ret;
-	}
-
-	return Reset() ? 0 : -1;
-}
-
-bool ICM45686::Reset()
-{
-	_state = STATE::RESET;
-	ScheduleClear();
-	ScheduleNow();
-	return true;
-}
-
-void ICM45686::exit_and_cleanup()
-{
-	I2CSPIDriverBase::exit_and_cleanup();
-}
-
-void ICM45686::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
-	PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled");
-
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-}
-
-int ICM45686::probe()
-{
-	for (int i = 0; i < 3; i++) {
-		const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
-
-		if (whoami != WHOAMI) {
-			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-			return PX4_ERROR;
-		}
-	}
-
-	return PX4_OK;
-}
-
-void ICM45686::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// DEVICE_CONFIG: Software reset configuration
-		RegisterWrite(Register::BANK_0::REG_MISC2, REG_MISC2_BIT::SOFT_RST);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
-		    && ((RegisterRead(Register::BANK_0::REG_MISC2) & Bit1) == 0x0)) {
-
-			// Wakeup accel and gyro and schedule remaining configuration
-			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then reset the FIFO
-			_state = STATE::FIFO_RESET;
-			ScheduleDelayed(1_ms);
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::FIFO_RESET:
-
-		_state = STATE::FIFO_READ;
-		FIFOReset();
-
-		ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-
-		break;
-
-	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = now;
-
-			bool success = false;
-
-			if (FIFORead(timestamp_sample)) {
-				success = true;
-
-				if (_failure_count > 0) {
-					_failure_count--;
-				}
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])) {
-					_last_config_check_timestamp = now;
-					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
-			}
-		}
-
-		break;
-	}
-}
-
-void ICM45686::ConfigureSampleRate(int sample_rate)
-{
-	// round down to the nearest FIFO sample dt
-	const float min_interval = FIFO_SAMPLE_DT;
-	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
-
-	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
-
-	// recompute FIFO empty interval (us) with actual gyro sample limit
-	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
-
-	ConfigureFIFOWatermark(_fifo_gyro_samples);
-}
-
-void ICM45686::ConfigureFIFOWatermark(uint8_t samples)
-{
-	// FIFO watermark threshold in number of bytes
-	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
-
-	for (auto &r : _register_bank0_cfg) {
-		if (r.reg == Register::BANK_0::FIFO_CONFIG1_0) {
-			// FIFO_WM[7:0]  FIFO_CONFIG2
-			r.set_bits = fifo_watermark_threshold & 0xFF;
-
-		} else if (r.reg == Register::BANK_0::FIFO_CONFIG1_1) {
-			// FIFO_WM[11:8] FIFO_CONFIG3
-			r.set_bits = (fifo_watermark_threshold >> 8) & 0xFF;
-		}
-	}
-}
-
-void ICM45686::ConfigureCLKIN()
-{
-	for (auto &r0 : _register_bank0_cfg) {
-		if (r0.reg == Register::BANK_0::RTC_CONFIG) {
-			r0.set_bits = RTC_CONFIG_BIT::RTC_MODE;
-		}
-	}
-
-	for (auto &r0 : _register_bank0_cfg) {
-		if (r0.reg == Register::BANK_0::IOC_PAD_SCENARIO_OVRD) {
-			r0.set_bits = PADS_INT2_CFG_OVRD | PADS_INT2_CFG_OVRD_CLKIN;
-		}
-	}
-}
-
-bool ICM45686::Configure()
-{
-	// Set it to little endian first, otherwise the chip doesn't match the manual
-	// which is just utterly confusing.
-	//uint8_t cmd[3] {
-	//    BANK_IPREG_TOP1,
-	//        SREG_CTRL,
-	//    SREG_CTRL_SREG_DATA_ENDIAN_SEL_BIT::SREG_CTRL_SREG_DATA_ENDIAN_SEL_BIG };
-	//transfer(cmd, cmd, sizeof(cmd));
-
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_bank0_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	// now check that all are configured
-	bool success = true;
-
-	for (const auto &reg_cfg : _register_bank0_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	// 20-bits data format used the only FSR settings that are operational
-	// are ±4000dps for gyroscope and ±32 for accelerometer
-	_px4_accel.set_range(32.f * CONSTANTS_ONE_G);
-	_px4_gyro.set_range(math::radians(4000.f));
-
-	return success;
-}
-
-template <typename T>
-bool ICM45686::RegisterCheck(const T &reg_cfg)
-{
-	bool success = true;
-
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
-
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_INFO("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
-
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_INFO("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
-
-	return success;
-}
-
-template <typename T>
-uint8_t ICM45686::RegisterRead(T reg)
-{
-	uint8_t cmd[2] {};
-	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
-	transfer(cmd, cmd, sizeof(cmd));
-	return cmd[1];
-}
-
-template <typename T>
-void ICM45686::RegisterWrite(T reg, uint8_t value)
-{
-	uint8_t cmd[2] { (uint8_t)reg, value };
-	transfer(cmd, cmd, sizeof(cmd));
-}
-
-template <typename T>
-void ICM45686::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
-
-	uint8_t val = (orig_val & ~clearbits) | setbits;
-
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
-}
-
-uint16_t ICM45686::FIFOReadCount()
-{
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNT_0) | DIR_READ;
-
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return 0;
-	}
-
-	// FIFO_COUNT_0 is supposed to contain the high bits and FIFO_COUNT_1 the low bits,
-	// according to the manual, however, the device is configured to little endianness
-	// which means FIFO and FIFO count are pre-swapped..
-	return combine(fifo_count_buf[2], fifo_count_buf[1]);
-}
-
-bool ICM45686::FIFORead(const hrt_abstime &timestamp_sample)
-{
-	const uint16_t fifo_packets = FIFOReadCount();
-
-	if (fifo_packets == 0) {
-		perf_count(_fifo_empty_perf);
-		return false;
-	}
-
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(sizeof(FIFOTransferBuffer), fifo_packets * sizeof(FIFO::DATA) + 1);
-
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
-	}
-
-	unsigned valid_samples = 0;
-
-	for (unsigned i = 0; i < transfer_size / sizeof(FIFO::DATA); i++) {
-		bool valid = true;
-
-		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
-		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
-
-		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
-			// FIFO sample empty if HEADER_MSG set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
-			// accel bit not set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
-			// gyro bit not set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
-			// Packet does not contain a new and valid extended 20-bit data
-			valid = false;
-
-		} else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) {
-			// Packet does not contain ODR timestamp
-			valid = false;
-
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
-			// accel ODR changed
-			valid = false;
-
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
-			// gyro ODR changed
-			valid = false;
-		}
-
-		if (valid) {
-			valid_samples++;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-			break;
-		}
-	}
-
-	if (valid_samples > 0) {
-		if (ProcessTemperature(buffer.f, valid_samples)) {
-			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-			return true;
-		}
-	}
-
-	return false;
-}
-
-void ICM45686::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
-
-	// Disable FIFO
-	RegisterClearBits(Register::BANK_0::FIFO_CONFIG3,
-			  FIFO_CONFIG3_BIT::FIFO_ES1_EN |
-			  FIFO_CONFIG3_BIT::FIFO_ES0_EN |
-			  FIFO_CONFIG3_BIT::FIFO_HIRES_EN |
-			  FIFO_CONFIG3_BIT::FIFO_GYRO_EN |
-			  FIFO_CONFIG3_BIT::FIFO_ACCEL_EN |
-			  FIFO_CONFIG3_BIT::FIFO_IF_EN);
-
-	// Disable FIFO by switching to bypass mode
-	RegisterSetAndClearBits(Register::BANK_0::FIFO_CONFIG0,
-				FIFO_CONFIG0_BIT::FIFO_MODE_BYPASS_SET,
-				FIFO_CONFIG0_BIT::FIFO_MODE_BYPASS_CLEAR);
-
-	// When the FIFO is disabled we can actually set the FIFO depth
-	RegisterSetBits(Register::BANK_0::FIFO_CONFIG0, FIFO_CONFIG0_BIT::FIFO_DEPTH_8K_SET);
-
-	// And then enable FIFO again
-	RegisterSetAndClearBits(Register::BANK_0::FIFO_CONFIG0, FIFO_CONFIG0_BIT::FIFO_MODE_STOP_ON_FULL_SET,
-				FIFO_CONFIG0_BIT::FIFO_MODE_STOP_ON_FULL_CLEAR);
-
-	// And enable again
-	RegisterSetBits(Register::BANK_0::FIFO_CONFIG3,
-			FIFO_CONFIG3_BIT::FIFO_HIRES_EN |
-			FIFO_CONFIG3_BIT::FIFO_GYRO_EN |
-			FIFO_CONFIG3_BIT::FIFO_ACCEL_EN |
-			FIFO_CONFIG3_BIT::FIFO_IF_EN);
-}
-
-void ICM45686::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-
-	// 19-bits of accelerometer data
-	bool scale_20bit = false;
-
-	// first pass
-	for (int i = 0; i < samples; i++) {
-
-
-		if (_enable_clock_input) {
-			// Swapped as device is in little endian by default.
-			const uint16_t timestamp_fifo = combine_uint(fifo[i].Timestamp_L, fifo[i].Timestamp_H);
-			accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
-
-		} else {
-			accel.dt = FIFO_TIMESTAMP_SCALING;
-		}
-
-		// 20 bit hires mode
-		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-		// Accel data is 18 bit ()
-		int32_t accel_x = reassemble_20bit(
-					  fifo[i].ACCEL_DATA_XL,
-					  fifo[i].ACCEL_DATA_XH,
-					  fifo[i].HIGHRES_X_LSB & 0xF0 >> 4);
-		int32_t accel_y = reassemble_20bit(
-					  fifo[i].ACCEL_DATA_YL,
-					  fifo[i].ACCEL_DATA_YH,
-					  fifo[i].HIGHRES_Y_LSB & 0xF0 >> 4);
-		int32_t accel_z = reassemble_20bit(
-					  fifo[i].ACCEL_DATA_ZL,
-					  fifo[i].ACCEL_DATA_ZH,
-					  fifo[i].HIGHRES_Z_LSB & 0xF0 >> 4);
-
-		// sample invalid if -524288
-		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
-
-			// It's not enough to check if any values are exceeding the
-			// int16 limits because there might be a rotation applied later.
-			// If a rotation is 45 degrees, the new component can be up to
-			// sqrt(2) longer than one component. This means the number has
-			// to be constrained to fit the int16 which then triggers
-			// clipping.
-			//
-			// Therefore, we set the limits at int16_max/min / sqrt(2) plus
-			// a bit of margin.
-			static constexpr int16_t max_accel = static_cast<int16_t>(INT16_MAX / sqrt(2.f)) - 100;
-			static constexpr int16_t min_accel = static_cast<int16_t>(INT16_MIN / sqrt(2.f)) + 100;
-
-			if (accel_x >= max_accel || accel_x <= min_accel) {
-				scale_20bit = true;
-			}
-
-			if (accel_y >= max_accel || accel_y <= min_accel) {
-				scale_20bit = true;
-			}
-
-			if (accel_z >= max_accel || accel_z <= min_accel) {
-				scale_20bit = true;
-			}
-
-			// least significant bit is always 0)
-			accel.x[accel.samples] = accel_x / 2;
-			accel.y[accel.samples] = accel_y / 2;
-			accel.z[accel.samples] = accel_z / 2;
-			accel.samples++;
-		}
-	}
-
-	if (!scale_20bit) {
-		// if highres enabled accel data is always 8192 LSB/g
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-
-	} else {
-		// 20 bit data scaled to 16 bit (2^4)
-		for (int i = 0; i < samples; i++) {
-			// 20 bit hires mode
-			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-			// Accel data is 18 bit ()
-			int16_t accel_x = combine(fifo[i].ACCEL_DATA_XL, fifo[i].ACCEL_DATA_XH);
-			int16_t accel_y = combine(fifo[i].ACCEL_DATA_YL, fifo[i].ACCEL_DATA_YH);
-			int16_t accel_z = combine(fifo[i].ACCEL_DATA_ZL, fifo[i].ACCEL_DATA_ZH);
-
-			accel.x[i] = accel_x;
-			accel.y[i] = accel_y;
-			accel.z[i] = accel_z;
-		}
-
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f * 8.0f);
-	}
-
-	// correct frame for publication
-	for (int i = 0; i < accel.samples; i++) {
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[i] = accel.x[i];
-		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
-		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
-	}
-
-	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-	}
-}
-
-void ICM45686::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = 0;
-
-	// 20-bits of gyroscope data
-	bool scale_20bit = false;
-
-	// first pass
-	for (int i = 0; i < samples; i++) {
-
-
-		if (_enable_clock_input) {
-			// Swapped as device is in little endian by default.
-			uint16_t timestamp_fifo = combine_uint(fifo[i].Timestamp_L, fifo[i].Timestamp_H);
-			gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
-
-		} else {
-			gyro.dt = FIFO_TIMESTAMP_SCALING;
-		}
-
-		// 20 bit hires mode
-		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
-		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_XL, fifo[i].GYRO_DATA_XH, fifo[i].HIGHRES_X_LSB & 0x0F);
-		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_YL, fifo[i].GYRO_DATA_YH, fifo[i].HIGHRES_Y_LSB & 0x0F);
-		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_ZL, fifo[i].GYRO_DATA_ZH, fifo[i].HIGHRES_Z_LSB & 0x0F);
-
-		// It's not enough to check if any values are exceeding the
-		// int16 limits because there might be a rotation applied later.
-		// If a rotation is 45 degrees, the new component can be up to
-		// sqrt(2) longer than one component. This means the number has
-		// to be constrained to fit the int16 which then triggers
-		// clipping.
-		//
-		// Therefore, we set the limits at int16_max/min / sqrt(2) plus
-		// a bit of margin.
-		static constexpr int16_t max_gyro = static_cast<int16_t>(INT16_MAX / sqrt(2.f)) - 100;
-		static constexpr int16_t min_gyro = static_cast<int16_t>(INT16_MIN / sqrt(2.f)) + 100;
-
-		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		gyro.x[gyro.samples] = gyro_x;
-		gyro.y[gyro.samples] = gyro_y;
-		gyro.z[gyro.samples] = gyro_z;
-		gyro.samples++;
-	}
-
-	if (!scale_20bit) {
-		// if highres enabled gyro data is always 131 LSB/dps
-		_px4_gyro.set_scale(math::radians(1.f / 131.f));
-
-	} else {
-		// 20 bit data scaled to 16 bit (2^4)
-		for (int i = 0; i < samples; i++) {
-			gyro.x[i] = combine(fifo[i].GYRO_DATA_XL, fifo[i].GYRO_DATA_XH);
-			gyro.y[i] = combine(fifo[i].GYRO_DATA_YL, fifo[i].GYRO_DATA_YH);
-			gyro.z[i] = combine(fifo[i].GYRO_DATA_ZL, fifo[i].GYRO_DATA_ZH);
-		}
-
-		_px4_gyro.set_scale(math::radians(1.f / 131.f * 16.0f));
-	}
-
-	// correct frame for publication
-	for (int i = 0; i < gyro.samples; i++) {
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro.x[i];
-		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
-		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
-	}
-
-	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-
-	if (gyro.samples > 0) {
-		_px4_gyro.updateFIFO(gyro);
-	}
-}
-
-bool ICM45686::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
-{
-	int16_t temperature[FIFO_MAX_SAMPLES];
-	float temperature_sum{0};
-
-	int valid_samples = 0;
-
-	for (int i = 0; i < samples; i++) {
-		// Swapped as device is in little endian by default.
-		const int16_t t = combine(fifo[i].TEMP_DATA_L, fifo[i].TEMP_DATA_H);
-
-		// sample invalid if -32768
-		if (t != -32768) {
-			temperature_sum += t;
-			temperature[valid_samples] = t;
-			valid_samples++;
-		}
-	}
-
-	if (valid_samples > 0) {
-		const float temperature_avg = temperature_sum / valid_samples;
-
-		for (int i = 0; i < valid_samples; i++) {
-			// temperature changing wildly is an indication of a transfer error
-			if (fabsf(temperature[i] - temperature_avg) > 1000) {
-				perf_count(_bad_transfer_perf);
-				return false;
-			}
-		}
-
-		// use average temperature reading
-		const float temp_c = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
-
-		if (PX4_ISFINITE(temp_c)) {
-			_px4_accel.set_temperature(temp_c);
-			_px4_gyro.set_temperature(temp_c);
-			return true;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	return false;
-}
@@ -1,165 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ICM45686.hpp
- *
- * Driver for the Invensense ICM45686 connected via SPI.
- *
- */
-
-#pragma once
-
-#include "InvenSense_ICM45686_registers.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/device/spi.h>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-#include <lib/geo/geo.h>
-#include <lib/perf/perf_counter.h>
-#include <px4_platform_common/atomic.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-
-using namespace InvenSense_ICM45686;
-
-class ICM45686 : public device::SPI, public I2CSPIDriver<ICM45686>
-{
-public:
-	ICM45686(const I2CSPIDriverConfig &config);
-	~ICM45686() override;
-
-	static void print_usage();
-
-	void RunImpl();
-
-	int init() override;
-	void print_status() override;
-
-private:
-	void exit_and_cleanup() override;
-
-	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
-
-	static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input
-
-	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_DATA) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	} __attribute__((packed));
-	// ensure padding is right
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
-
-	struct register_bank0_config_t {
-		Register::BANK_0 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Reset();
-
-	bool Configure();
-	void ConfigureSampleRate(int sample_rate);
-	void ConfigureFIFOWatermark(uint8_t samples);
-	void ConfigureCLKIN();
-
-	template <typename T> bool RegisterCheck(const T &reg_cfg);
-	template <typename T> uint8_t RegisterRead(T reg);
-	template <typename T> void RegisterWrite(T reg, uint8_t value);
-	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
-	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
-	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
-
-	uint16_t FIFOReadCount();
-	bool FIFORead(const hrt_abstime &timestamp_sample);
-	void FIFOReset();
-
-	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
-
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
-
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
-	hrt_abstime _temperature_update_timestamp{0};
-	int _failure_count{0};
-
-	bool _enable_clock_input{false};
-	float _input_clock_freq{0.f};
-
-	bool _data_ready_interrupt_enabled{false};
-
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		FIFO_RESET,
-		FIFO_READ,
-	} _state{STATE::RESET};
-
-	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
-
-	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{9};
-	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
-		{ Register::BANK_0::INT1_CONFIG0, 0, 0},
-		{ Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
-
-		{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_UI_FS_SEL_4000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_6400_HZ_SET, GYRO_CONFIG0_BIT::GYRO_UI_FS_SEL_4000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_6400_HZ_CLEAR },
-		{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_32_G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_6400_HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_32_G_CLEAR | ACCEL_CONFIG0_BIT::ACCEL_ODR_6400_HZ_CLEAR },
-		{ Register::BANK_0::FIFO_CONFIG4, 0, FIFO_CONFIG4_BIT::FIFO_COMP_EN },
-		{ Register::BANK_0::FIFO_CONFIG0, FIFO_CONFIG0_BIT::FIFO_MODE_STOP_ON_FULL_SET | FIFO_CONFIG0_BIT::FIFO_DEPTH_8K_SET, FIFO_CONFIG0_BIT::FIFO_MODE_STOP_ON_FULL_CLEAR | FIFO_CONFIG0_BIT::FIFO_DEPTH_8K_CLEAR },
-		{ Register::BANK_0::FIFO_CONFIG3, FIFO_CONFIG3_BIT::FIFO_HIRES_EN | FIFO_CONFIG3_BIT::FIFO_GYRO_EN | FIFO_CONFIG3_BIT::FIFO_ACCEL_EN | FIFO_CONFIG3_BIT::FIFO_IF_EN, 0 },
-
-		{ Register::BANK_0::RTC_CONFIG, 0, 0}, // RTC_MODE[5] set at runtime
-		{ Register::BANK_0::IOC_PAD_SCENARIO_OVRD, 0, 0}, // PADS_INT2_CFG_OVRD and PADS_INT2_CFG_OVRD_VAL set at runtime
-	};
-};
@@ -1,266 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file InvenSense_ICM45686_registers.hpp
- *
- * Invensense ICM-45686 registers.
- *
- */
-
-#pragma once
-
-#include <cstdint>
-#include <cstddef>
-
-namespace InvenSense_ICM45686
-{
-// TODO: move to a central header
-static constexpr uint8_t Bit0 = (1 << 0);
-static constexpr uint8_t Bit1 = (1 << 1);
-static constexpr uint8_t Bit2 = (1 << 2);
-static constexpr uint8_t Bit3 = (1 << 3);
-static constexpr uint8_t Bit4 = (1 << 4);
-static constexpr uint8_t Bit5 = (1 << 5);
-static constexpr uint8_t Bit6 = (1 << 6);
-static constexpr uint8_t Bit7 = (1 << 7);
-
-static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
-static constexpr uint8_t DIR_READ = 0x80;
-
-static constexpr uint8_t WHOAMI = 0xE9;
-
-static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
-static constexpr float TEMPERATURE_OFFSET = 25.f; // C
-
-namespace Register
-{
-
-enum class BANK_0 : uint8_t {
-	PWR_MGMT0 = 0x10,
-	FIFO_COUNT_0 = 0x12,
-	FIFO_COUNT_1 = 0x13,
-	FIFO_DATA = 0x14,
-
-	INT1_CONFIG0 = 0x16,
-	INT1_CONFIG1 = 0x17,
-	INT1_CONFIG2 = 0x18,
-	INT1_STATUS0 = 0x19,
-	ACCEL_CONFIG0 = 0x1B,
-	GYRO_CONFIG0 = 0x1C,
-	FIFO_CONFIG0 = 0x1D,
-	FIFO_CONFIG1_0 = 0x1E,
-	FIFO_CONFIG1_1 = 0x1F,
-	FIFO_CONFIG2 = 0x20,
-	FIFO_CONFIG3 = 0x21,
-	FIFO_CONFIG4 = 0x22,
-	RTC_CONFIG = 0x26,
-	DMP_EXT_SEN_ODR_CFG = 0x27,
-	EDMP_APEX_EN0 = 0x29,
-	EDMP_APEX_EN1 = 0x2A,
-	APEX_BUFFER_MGMT = 0x2B,
-	INTF_CONFIG0 = 0x2C,
-	INTF_CONFIG1_OVRD = 0x2D,
-	INTF_AUX_CONFIG = 0x2E,
-	IOC_PAD_SCENARIO = 0x2F,
-	IOC_PAD_SCENARIO_AUX_OVRD = 0x30,
-	IOC_PAD_SCENARIO_OVRD = 0x31,
-	DRIVE_CONFIG0 = 0x32,
-	DRIVE_CONFIG1 = 0x33,
-	DRIVE_CONFIG2 = 0x34,
-	INT_APEX_CONFIG1 = 0x3a,
-	INT_APEX_STATUS0 = 0x3b,
-	INT_APEX_STATUS1 = 0x3c,
-
-	INT2_CONFIG0 = 0x56,
-	INT2_CONFIG1 = 0x57,
-	INT2_CONFIG2 = 0x58,
-	INT2_STATUS0 = 0x59,
-
-	WHO_AM_I = 0x72,
-	REG_MISC2 = 0x7F,
-};
-
-};
-
-//---------------- BANK0 Register bits
-
-// PWR_MGMT0
-enum PWR_MGMT0_BIT : uint8_t {
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
-};
-
-enum INT1_STATUS0 : uint8_t {
-	INT1_STATUS_RESET_DONE = Bit7,
-	INT1_STATUS_AUX1_AGC = Bit6,
-	INT1_STATUS_AP_AGC_RDY = Bit5,
-	INT1_STATUS_AP_FSYNC = Bit4,
-	INT1_STATUS_AP_AUX1_DRDY = Bit3,
-	INT1_STATUS_AP_DRDY = Bit2,
-	INT1_STATUS_FIFO_THS = Bit1,
-	INT1_STATUS_FIFO_FULL = Bit0,
-};
-
-enum ACCEL_CONFIG0_BIT : uint8_t {
-	ACCEL_UI_FS_SEL_32_G_SET = 0,
-	ACCEL_UI_FS_SEL_32_G_CLEAR = Bit6 | Bit5 | Bit4,
-	ACCEL_UI_FS_SEL_16_G_SET = Bit4,
-	ACCEL_UI_FS_SEL_16_G_CLEAR = Bit6 | Bit5,
-	ACCEL_UI_FS_SEL_8_G_SET = Bit5,
-	ACCEL_UI_FS_SEL_8_G_CLEAR = Bit6 | Bit4,
-	ACCEL_ODR_6400_HZ_SET = Bit0 | Bit1,
-	ACCEL_ODR_6400_HZ_CLEAR = Bit2,
-	ACCEL_ODR_3200_HZ_SET = Bit2,
-	ACCEL_ODR_3200_HZ_CLEAR = Bit0 | Bit1,
-	ACCEL_ODR_1600_HZ_SET = Bit2 | Bit0,
-	ACCEL_ODR_1600_HZ_CLEAR = Bit1,
-	ACCEL_ODR_800_HZ_SET = Bit2 | Bit1,
-	ACCEL_ODR_800_HZ_CLEAR = Bit0,
-};
-
-enum GYRO_CONFIG0_BIT : uint8_t {
-	GYRO_UI_FS_SEL_4000_DPS_SET = 0,
-	GYRO_UI_FS_SEL_4000_DPS_CLEAR = Bit7 | Bit6 | Bit5 | Bit4,
-	GYRO_UI_FS_SEL_2000_DPS_SET = Bit4,
-	GYRO_UI_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
-	GYRO_UI_FS_SEL_1000_DPS_SET = Bit5,
-	GYRO_UI_FS_SEL_1000_DPS_CLEAR = Bit7 | Bit6 | Bit4,
-	GYRO_ODR_6400_HZ_SET = Bit0 | Bit1,
-	GYRO_ODR_6400_HZ_CLEAR = Bit2,
-	GYRO_ODR_3200_HZ_SET = Bit2,
-	GYRO_ODR_3200_HZ_CLEAR = Bit0 | Bit1,
-	GYRO_ODR_1600_HZ_SET = Bit2 | Bit0,
-	GYRO_ODR_1600_HZ_CLEAR = Bit1,
-	GYRO_ODR_800_HZ_SET = Bit2 | Bit1,
-	GYRO_ODR_800_HZ_CLEAR = Bit0,
-};
-
-enum FIFO_CONFIG0_BIT : uint8_t {
-	FIFO_MODE_BYPASS_SET = 0,
-	FIFO_MODE_BYPASS_CLEAR = Bit6 | Bit7,
-	FIFO_MODE_STREAM_SET = Bit6,
-	FIFO_MODE_STREAM_CLEAR = Bit7,
-	FIFO_MODE_STOP_ON_FULL_SET = Bit7,
-	FIFO_MODE_STOP_ON_FULL_CLEAR = Bit6,
-	FIFO_DEPTH_2K_SET = Bit0 | Bit1 | Bit2,
-	FIFO_DEPTH_2K_CLEAR = Bit3 | Bit4,
-	FIFO_DEPTH_8K_SET = Bit0 | Bit1 | Bit2 | Bit3 | Bit4,
-	FIFO_DEPTH_8K_CLEAR = 0,
-};
-
-enum FIFO_CONFIG2_BIT : uint8_t {
-	FIFO_FLUSH = Bit7,
-	FIFO_WR_WM_GT_TH_EQUAL = 0,
-	FIFO_WR_WM_GT_TH_GREATER_THAN = Bit3,
-};
-
-enum FIFO_CONFIG3_BIT : uint8_t {
-	FIFO_ES1_EN = Bit5, // External sensor 1 data insertion into FIFO frame
-	FIFO_ES0_EN = Bit4, // External sensor 0 data insertion into FIFO frame
-	FIFO_HIRES_EN = Bit3, // High resolution accel and gyro data insertion into FIFO frame
-	FIFO_GYRO_EN = Bit2, // Gyro data insertion into FIFO frame
-	FIFO_ACCEL_EN = Bit1, // Accel data insertion into FIFO frame
-	FIFO_IF_EN = Bit0, // Enable FIFO
-};
-
-enum FIFO_CONFIG4_BIT : uint8_t {
-	FIFO_COMP_EN = Bit2, // FIFO compression enabled
-	FIFO_TMST_FSYNC_EN = Bit1, // Timestamp/FSYNC data inserted into FIFO frame
-};
-
-enum RTC_CONFIG_BIT : uint8_t {
-	RTC_ALIGN = Bit6, // Re-align command is generated by writing 1 to this bit
-	RTC_MODE = Bit5, // 0: RTC functionality not enabled, 1: RTC functionality enabled
-};
-
-enum IOC_PAD_SCENARIO_OVRD_BIT : uint8_t {
-	PADS_INT2_CFG_OVRD = Bit2, // Override enable for PADS_INT2_CFG, 0: disable, 1: enable
-	PADS_INT2_CFG_OVRD_INT2 = 0,
-	PADS_INT2_CFG_OVRD_FSYNC = Bit0,
-	PADS_INT2_CFG_OVRD_CLKIN = Bit1,
-};
-
-enum REG_MISC2_BIT : uint8_t {
-	SOFT_RST = Bit1, // 1: Triggers soft reset operation
-};
-
-
-// IPREG_TOP1
-//static constexpr uint8_t BANK_IPREG_TOP1 = 0xA2;
-//static constexpr uint8_t SREG_CTRL = 0x67;
-//enum SREG_CTRL_SREG_DATA_ENDIAN_SEL_BIT : uint8_t {
-//   SREG_CTRL_SREG_DATA_ENDIAN_SEL_BIG = Bit1, // big endian as documented (instead of default little endian)
-//};
-
-
-namespace FIFO
-{
-static constexpr size_t SIZE = 8192;
-
-struct DATA {
-	uint8_t FIFO_Header;
-	uint8_t ACCEL_DATA_XH; // Accel X [19:12]
-	uint8_t ACCEL_DATA_XL; // Accel X [11:4]
-	uint8_t ACCEL_DATA_YH; // Accel Y [19:12]
-	uint8_t ACCEL_DATA_YL; // Accel Y [11:4]
-	uint8_t ACCEL_DATA_ZH; // Accel Z [19:12]
-	uint8_t ACCEL_DATA_ZL; // Accel Z [11:4]
-	uint8_t GYRO_DATA_XH;  // Gyro X [19:12]
-	uint8_t GYRO_DATA_XL;  // Gyro X [11:4]
-	uint8_t GYRO_DATA_YH;  // Gyro Y [19:12]
-	uint8_t GYRO_DATA_YL;  // Gyro Y [11:4]
-	uint8_t GYRO_DATA_ZH;  // Gyro Z [19:12]
-	uint8_t GYRO_DATA_ZL;  // Gyro Z [11:4]
-	uint8_t TEMP_DATA_H;    // Temperature[15:8]
-	uint8_t TEMP_DATA_L;    // Temperature[7:0]
-	uint8_t Timestamp_H;   // Timestamp[15:8]
-	uint8_t Timestamp_L;   // Timestamp[7:0]
-	uint8_t HIGHRES_X_LSB; // Accel X LSB [3:0] Gyro X LSB [3:0]
-	uint8_t HIGHRES_Y_LSB; // Accel Y LSB [3:0] Gyro Y LSB [3:0]
-	uint8_t HIGHRES_Z_LSB; // Accel Z LSB [3:0] Gyro Z LSB [3:0]
-};
-
-// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
-enum FIFO_HEADER_BIT : uint8_t {
-	HEADER_MSG             = Bit7, // 1: FIFO is empty
-	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
-	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
-	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
-	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp
-	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
-	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
-};
-
-}
-} // namespace InvenSense_ICM42688P
@@ -1,5 +0,0 @@
-menuconfig DRIVERS_IMU_INVENSENSE_ICM45686
-	bool "icm45686"
-	default n
-	---help---
-		Enable support for icm45686
@@ -1,92 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "ICM45686.hpp"
-
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
-
-void ICM45686::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-extern "C" int icm45686_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = ICM45686;
-	BusCLIArguments cli{false, true};
-	cli.default_spi_frequency = SPI_SPEED;
-
-	while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) {
-		switch (ch) {
-		case 'C':
-			cli.custom1 = atoi(cli.optArg());
-			break;
-
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
-
-	const char *verb = cli.optArg();
-
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM45686);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}
@@ -115,8 +115,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 				      GPSGROUP_NONE)
 	   ) {
 		// TIMEGROUP_TIMESTARTUP
-		uint64_t time_startup = VnUartPacket_extractUint64(packet);
-		(void)time_startup;
+		//uint64_t time_startup = VnUartPacket_extractUint64(packet);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

+
+
+
 	ScheduleDelayed(100_ms);
 }

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

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

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

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


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

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

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

 	InitializeCriticalSection(&cs->handle);

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

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

 	DeleteCriticalSection(&cs->handle);

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

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

 	EnterCriticalSection(&cs->handle);

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

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

 	LeaveCriticalSection(&cs->handle);

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

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

 	return E_UNKNOWN;

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

 	return VnEvent_waitUs(e, timeoutMs * 1000);

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

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

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

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

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

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

 	serialport->handle = NULL;

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

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

 	serialport->handle = NULL;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,5 +1,5 @@
 /* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
-#if defined(__linux__) || defined(__NUTTX__)
+#ifdef __linux__
 	/* Works for Ubuntu 15.10 */
 	#define _POSIX_C_SOURCE 199309L
 #elif defined __CYGWIN__
--- a/Show More
+++ b/Show More