DNM px4_fmu-v5x Using Probes

DNM Prioity Inheritance Dumping added to logger
DNM Probes init PWM Disabled
2026-06-11 22:40:04 +08:00 · 2023-03-09 05:57:52 -08:00 · 2023-03-09 05:57:52 -08:00 · 2023-03-09 05:57:52 -08:00 · 2023-03-09 05:57:52 -08:00 · 2023-03-06 18:34:01 -05:00
238 changed files with 5995 additions and 4791 deletions
@@ -110,6 +110,8 @@ pipeline {
                     "raspberrypi_pico_default",
                     "sky-drones_smartap-airlink_default",
                     "spracing_h7extreme_default",
+                     "thepeach_k1_default",
+                     "thepeach_r1_default",
                     "uvify_core_default"
            ],
            image: docker_images.nuttx,
@@ -44,81 +44,88 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con

 ## Maintenance Team

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

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

 ## Supported Hardware

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

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

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

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

 ## Project Roadmap

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

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

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

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

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

 param set-default SIM_GZ_EN 1

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

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

 param set-default SIM_GZ_EN 1

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

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

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
 param set-default SENS_EN_ARSPDSIM 1

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

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
 param set-default SENS_EN_ARSPDSIM 1

 # TODO: Enable motor failure detection when the
@@ -0,0 +1,12 @@
+#!/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}
@@ -74,6 +74,7 @@ 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
@@ -10,9 +10,18 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"

 	if simulator_sih start; then

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

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

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

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

 		if gz_bridge start -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
-			sensor_baro_sim start
-			sensor_gps_sim start
-			sensor_mag_sim start
+			if param compare -s SENS_EN_BAROSIM 1
+			then
+				sensor_baro_sim start
+			fi
+			if param compare -s SENS_EN_GPSSIM 1
+			then
+				sensor_gps_sim start
+			fi
+			if param compare -s SENS_EN_MAGSIM 1
+			then
+				sensor_mag_sim start
+			fi
 			if param compare -s SENS_EN_ARSPDSIM 1
 			then
 				sensor_airspeed_sim start
@@ -36,6 +36,7 @@ add_subdirectory(airframes)
 px4_add_romfs_files(
 	rc.airship_apps
 	rc.airship_defaults
+	rc.autostart_ext
 	rc.balloon_apps
 	rc.balloon_defaults
 	rc.boat_defaults
@@ -122,16 +122,12 @@ 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,14 +54,6 @@ 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,7 +17,6 @@ param set-default COM_POS_FS_DELAY 5

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

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

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

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

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

 	print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name))
 	print('\tbuf.offset += sizeof(topic.{:});'.format(field_name))
@@ -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 = int(args[1], 0)
+        default_val = self._get_int(args[1])
        description = self._get_string(args[4])
        if self._is_bool_true(args[5]):
            self._usage_string += "     [-%s <val>]  %s\n" % (option_char, description)
@@ -214,6 +214,9 @@ 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] == '"'

@@ -307,6 +310,8 @@ 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
@@ -316,6 +321,9 @@ 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
@@ -379,6 +387,15 @@ 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,6 +84,7 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i
 	gdb \
 	git \
 	lcov \
+	libfuse2 \
 	libxml2-dev \
 	libxml2-utils \
 	make \
@@ -182,6 +183,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 			echo "${NUTTX_GCC_VERSION} path already set.";
 		else
 			echo $exportline >> $HOME/.profile;
+			source $HOME/.profile; # Allows to directly build NuttX targets in the same terminal
 		fi
 	fi
 fi
@@ -217,33 +219,34 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	# Set Java 11 as default
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")

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

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

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

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

 if param greater -s UAVCAN_ENABLE 0
 then
@@ -22,4 +23,11 @@ then
 	ifup can1
 	ifup can2
 	ifup can3
-fi
+fi
+if param greater -s CYPHAL_ENABLE 0
+then
+	ifup can0
+	ifup can1
+	ifup can2
+	ifup can3
+fi
@@ -29,7 +29,7 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
@@ -4,3 +4,4 @@ CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
 CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
 CONFIG_DRIVERS_CYPHAL=y
+CONFIG_CYPHAL_ESC_CONTROLLER=y
@@ -75,6 +75,7 @@ CONFIG_BOARD_ASSERT_RESET_VALUE=0
 CONFIG_BOARD_CRASHDUMP=y
 CONFIG_BOARD_LOOPSPERMSEC=22114
 CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BOARD_USE_PROBES=y
 CONFIG_BUILTIN=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
@@ -47,6 +47,7 @@ then
 	fi
 fi

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

 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
@@ -5,13 +5,8 @@ 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
+# TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug
@@ -128,6 +128,7 @@ set(msg_files
 	MountOrientation.msg
 	ModeCompleted.msg
 	NavigatorMissionItem.msg
+	NormalizedUnsignedSetpoint.msg
 	NpfgStatus.msg
 	ObstacleDistance.msg
 	OffboardControlMode.msg
@@ -178,6 +179,7 @@ set(msg_files
 	TaskStackInfo.msg
 	TecsStatus.msg
 	TelemetryStatus.msg
+	TiltrotorExtraControls.msg
 	TimesyncStatus.msg
 	TrajectoryBezier.msg
 	TrajectorySetpoint.msg
@@ -218,7 +220,6 @@ set(msg_files
 	VehicleTrajectoryBezier.msg
 	VehicleTrajectoryWaypoint.msg
 	VtolVehicleStatus.msg
-	WheelEncoders.msg
 	Wind.msg
 	YawEstimatorStatus.msg
 )
@@ -5,12 +5,14 @@ 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)
@@ -28,7 +28,6 @@ bool local_position_invalid           # Local position estimate invalid
 bool local_position_invalid_relaxed   # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
 bool local_velocity_invalid           # Local velocity estimate invalid
 bool global_position_invalid          # Global position estimate invalid
-bool gps_position_invalid             # GPS position invalid
 bool auto_mission_missing             # No mission available
 bool offboard_control_signal_lost     # Offboard signal lost
 bool home_position_invalid            # No home position available
@@ -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			 # flap position
+float32 flaps			 # position of flaps switch/knob/lever [-1, 1]

 float32 aux1
 float32 aux2
@@ -0,0 +1,5 @@
+uint64 timestamp        		# time since system start (microseconds)
+
+float32 normalized_setpoint          	# [0, 1]
+
+# TOPICS flaps_setpoint spoilers_setpoint
@@ -28,8 +28,3 @@ float32 throttle_trim			# estimated throttle value [0,1] required to fly level a
 uint8 mode
 uint8 TECS_MODE_NORMAL = 0
 uint8 TECS_MODE_UNDERSPEED = 1
-uint8 TECS_MODE_TAKEOFF = 2
-uint8 TECS_MODE_LAND = 3
-uint8 TECS_MODE_LAND_THROTTLELIM = 4
-uint8 TECS_MODE_BAD_DESCENT = 5
-uint8 TECS_MODE_CLIMBOUT = 6
@@ -0,0 +1,4 @@
+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,14 +17,4 @@ 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
@@ -1,5 +0,0 @@
-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,7 +99,10 @@ __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", 1632, -21};
-static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22};
-static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23};
-static constexpr wq_config_t ttyS3{"wq:ttyS3", 1632, -24};
-static constexpr wq_config_t ttyS4{"wq:ttyS4", 1632, -25};
-static constexpr wq_config_t ttyS5{"wq:ttyS5", 1632, -26};
-static constexpr wq_config_t ttyS6{"wq:ttyS6", 1632, -27};
-static constexpr wq_config_t ttyS7{"wq:ttyS7", 1632, -28};
-static constexpr wq_config_t ttyS8{"wq:ttyS8", 1632, -29};
-static constexpr wq_config_t ttyS9{"wq:ttyS9", 1632, -30};
-static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1632, -31};
-static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1632, -32};
+static constexpr wq_config_t ttyS0{"wq:ttyS0", 1728, -21};
+static constexpr wq_config_t ttyS1{"wq:ttyS1", 1728, -22};
+static constexpr wq_config_t ttyS2{"wq:ttyS2", 1728, -23};
+static constexpr wq_config_t ttyS3{"wq:ttyS3", 1728, -24};
+static constexpr wq_config_t ttyS4{"wq:ttyS4", 1728, -25};
+static constexpr wq_config_t ttyS5{"wq:ttyS5", 1728, -26};
+static constexpr wq_config_t ttyS6{"wq:ttyS6", 1728, -27};
+static constexpr wq_config_t ttyS7{"wq:ttyS7", 1728, -28};
+static constexpr wq_config_t ttyS8{"wq:ttyS8", 1728, -29};
+static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30};
+static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31};
+static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32};

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

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

+// Logger watchdog priority, triggered when a task busy-loops (and
+// restored after a short time).
+// The priority is a trade-off between:
+// - ability to capture any busy-looping task below this priority
+// - not having a negative impact on the system itself
+#define SCHED_PRIORITY_LOG_WATCHDOG		(PX4_WQ_HP_BASE - 6)
+
 // Position controllers typically are in a blocking wait on estimator data
 // so when new sensor data is available they will run last. Keeping them
 // on a high priority ensures that they are the first process to be run
@@ -105,7 +105,8 @@ 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 bool is_server_running(int instance, bool server);
+static int get_server_running(int instance, bool *is_running);
+static int set_server_running(int instance);
 static void print_usage();
 static bool dir_exists(const std::string &path);
 static bool file_exists(const std::string &name);
@@ -124,6 +125,7 @@ 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;
@@ -131,6 +133,9 @@ 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];
@@ -146,8 +151,6 @@ 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 */
@@ -160,15 +163,16 @@ int main(int argc, char **argv)

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

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

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

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

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

 		bool working_directory_default = false;

-		int instance = 0;
 		bool instance_provided = false;

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

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

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

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

-		int ret = create_symlinks_if_needed(data_path);
+		if (server_is_running) {
+			// 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;
+		}
+
+		ret = create_symlinks_if_needed(data_path);

 		if (ret != PX4_OK) {
 			return ret;
@@ -343,6 +353,13 @@ 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) {
@@ -618,39 +635,73 @@ void print_usage()
 	printf("        e.g.: px4-commander status\n");
 }

-bool is_server_running(int instance, bool server)
+int get_server_running(int instance, bool *is_server_running)
 {
 	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("is_server_running: failed to create lock file: %s, reason=%s", file_lock_path.c_str(), strerror(errno));
-		return false;
+		PX4_ERR("%s: failed to create lock file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
+		return PX4_ERROR;
 	}

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

-	// 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;
+	// 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;

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

-	if (result || !server) {
+	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;
 		close(fd);
 	}

-	// note: server leaks the file handle once, on purpose, in order to keep the lock on the file until the process terminates.
+	// note: server leaks the file handle, 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.

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

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

 			} else {
-				//px4_clock_gettimemap[task_index].argv_storage[i], argv[i]);
+				strcpy(taskmap[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;
 		}
@@ -420,13 +421,13 @@ int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
 	return 0;
 }

-int px4_prctl(int option, const char *arg2, pthread_t pid)
+int px4_prctl(int option, const char *arg2, px4_task_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 == pid) {
+		if (taskmap[i].isused && taskmap[i].tid == (pthread_t) pid) {
 			rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2);
 			return rv;
 		}
@@ -65,12 +65,13 @@ 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);
@@ -152,6 +153,8 @@ 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;
@@ -336,9 +339,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
@@ -585,7 +588,7 @@ int ModalIo::custom_command(int argc, char *argv[])
 	}

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

 	} else if (!strcmp(verb, "version")) {
-		if (esc_id < 4) {
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
 			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;
@@ -610,7 +613,7 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	} else if (!strcmp(verb, "version-ext")) {
-		if (esc_id < 4) {
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
 			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;
@@ -623,14 +626,14 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	} else if (!strcmp(verb, "tone")) {
-		if (0 < esc_id && esc_id < 16) {
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
 			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 mask, use 1-15");
+			print_usage("Invalid ESC ID, use 0-3");
 			return 0;
 		}

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

 	}  else if (!strcmp(verb, "rpm")) {
-		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;
+		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};
 			uint8_t id_fb = 0;

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

-			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;
-				}
+			} else {
+				rate_req[esc_id] = rate;
+				id_fb = esc_id;
 			}

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

-			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("feedback id debug: %i", id_fb);
 			PX4_INFO("Sending UART ESC RPM command %i", rate);

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

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

 	} else if (!strcmp(verb, "pwm")) {
-		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;
+		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};
 			uint8_t id_fb = 0;

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

-			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;
-				}
+			} else {
+				rate_req[esc_id] = rate;
+				id_fb = esc_id;
 			}

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

-			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("feedback id debug: %i", id_fb);
 			PX4_INFO("Sending UART ESC power command %i", rate);

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

 		} else {
@@ -1157,8 +1114,7 @@ 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 (MODALAI_PUBLISH_ESC_STATUS) {
+	if (_parameters.verbose_logging) {
 		actuator_outputs_s actuator_outputs{};
 		actuator_outputs.noutputs = num_outputs;

@@ -1169,9 +1125,10 @@ 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);

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

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

 				} else {
@@ -1433,19 +1392,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', 1, 1, 15, "ESC ID bitmask, 1-15", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", 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', 1, 1, 15, "ESC ID bitmask, 1-15", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", 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', 1, 1, 15, "ESC ID bitmask, 1-15", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", 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,6 +143,7 @@ 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 {
@@ -188,7 +189,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,3 +201,16 @@ 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,6 +53,7 @@
 #include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_test.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
@@ -69,69 +70,38 @@ public:
 	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;

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

 	~UavcanEscController() {};

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 	_pub_manager.printInfo();

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

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


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

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

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

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

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

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

 public:

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

-			if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) {
+			if (strcmp(subject_name, sub.subject_name) == 0 && instance == sub.instance) {
 				found_subscriber = true;
 				break;
 			}
@@ -126,7 +126,7 @@ private:
 			{
 				return new UavcanEscSubscriber(handle, pmgr, 0);
 			},
-			"esc",
+			"udral.esc",
 			0
 		},
 #endif
@@ -136,7 +136,7 @@ private:
 			{
 				return new UavcanGnssSubscriber(handle, pmgr, 0);
 			},
-			"gps",
+			"udral.gps",
 			0
 		},
 #endif
@@ -146,7 +146,7 @@ private:
 			{
 				return new UavcanGnssSubscriber(handle, pmgr, 1);
 			},
-			"gps",
+			"udral.gps",
 			1
 		},
 #endif
@@ -156,7 +156,7 @@ private:
 			{
 				return new UavcanBmsSubscriber(handle, pmgr, 0);
 			},
-			"energy_source",
+			"udral.energy_source",
 			0
 		},
 #endif
@@ -166,7 +166,7 @@ private:
 			{
 				return new UavcanLegacyBatteryInfoSubscriber(handle, pmgr, 0);
 			},
-			"legacy_bms",
+			"udral.legacy_bms",
 			0
 		},
 #endif
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,13 +31,20 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__roboclaw
-	MAIN roboclaw
+	MODULE drivers__distance_sensor__lightware_sf45_serial
+	MAIN lightware_sf45_serial
 	COMPILE_FLAGS
 	SRCS
-		roboclaw_main.cpp
-		RoboClaw.cpp
+		lightware_sf45_serial.cpp
+		lightware_sf45_serial.hpp
+		lightware_sf45_serial_main.cpp
+	DEPENDS
+		drivers_rangefinder
+		px4_work_queue
 	MODULE_CONFIG
 		module.yaml
 	)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

-
-
-
 	ScheduleDelayed(100_ms);
 }

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

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

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

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


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

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

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

 	InitializeCriticalSection(&cs->handle);

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

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

 	DeleteCriticalSection(&cs->handle);

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

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

 	EnterCriticalSection(&cs->handle);

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

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

 	LeaveCriticalSection(&cs->handle);

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

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

 	return E_UNKNOWN;

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

 	return VnEvent_waitUs(e, timeoutMs * 1000);

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

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

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

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

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

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

 	serialport->handle = NULL;

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

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

 	serialport->handle = NULL;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,5 +1,5 @@
 /* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
-#ifdef __linux__
+#if defined(__linux__) || defined(__NUTTX__)
 	/* Works for Ubuntu 15.10 */
 	#define _POSIX_C_SOURCE 199309L
 #elif defined __CYGWIN__
@@ -40,5 +40,5 @@ px4_add_module(
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
+		mixer_module
 	)
-
@@ -319,5 +319,19 @@ px4io driver is used for main ones.

 extern "C" __EXPORT int pwm_out_main(int argc, char *argv[])
 {
+#if defined(CONFIG_BOARD_USE_PROBES)
+#pragma message "warning PWM will not Function when CONFIG_BOARD_USE_PROBES is set"
+  PROBE_INIT(0xff);
+  PROBE(1,1);
+  PROBE(2,1);
+  PROBE(3,1);
+  PROBE(4,1);
+  PROBE(5,1);
+  PROBE(6,1);
+  PROBE(7,1);
+  PROBE(8,1);
+  return 0;
+#else
 	return PWMOut::main(argc, argv);
+#endif
 }
@@ -50,7 +50,7 @@
 #include <uORB/topics/qshell_retval.h>
 #include <drivers/drv_hrt.h>

-#define MAX_ARGS 8 // max number of whitespace separated args after app name
+#define MAX_ARGS 16 // max number of whitespace separated args after app name

 px4::AppState QShell::appState;

@@ -1,5 +0,0 @@
-menuconfig DRIVERS_ROBOCLAW
-	bool "roboclaw"
-	default n
-	---help---
-		Enable support for roboclaw
@@ -1,610 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 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 RoboClaw.cpp
- *
- * RoboClaw Motor Driver
- *
- * references:
- * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
- *
- */
-
-#include "RoboClaw.hpp"
-#include <poll.h>
-#include <stdio.h>
-#include <math.h>
-#include <string.h>
-#include <fcntl.h>
-#include <termios.h>
-
-#include <systemlib/err.h>
-#include <systemlib/mavlink_log.h>
-
-#include <uORB/Publication.hpp>
-#include <drivers/drv_hrt.h>
-#include <math.h>
-
-// The RoboClaw has a serial communication timeout of 10ms.
-// Add a little extra to account for timing inaccuracy
-#define TIMEOUT_US 10500
-
-// If a timeout occurs during serial communication, it will immediately try again this many times
-#define TIMEOUT_RETRIES 1
-
-// If a timeout occurs while disarmed, it will try again this many times. This should be a higher number,
-// because stopping when disarmed is pretty important.
-#define STOP_RETRIES 10
-
-// Number of bytes returned by the Roboclaw when sending command 78, read both encoders
-#define ENCODER_MESSAGE_SIZE 10
-
-// Number of bytes for commands 18 and 19, read speeds.
-#define ENCODER_SPEED_MESSAGE_SIZE 7
-
-bool RoboClaw::taskShouldExit = false;
-
-RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam):
-	_uart(0),
-	_uart_set(),
-	_uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US},
-	_actuatorsSub(-1),
-	_lastEncoderCount{0, 0},
-	_encoderCounts{0, 0},
-	_motorSpeeds{0, 0}
-
-{
-	_param_handles.actuator_write_period_ms = 	param_find("RBCLW_WRITE_PER");
-	_param_handles.encoder_read_period_ms = 	param_find("RBCLW_READ_PER");
-	_param_handles.counts_per_rev = 			param_find("RBCLW_COUNTS_REV");
-	_param_handles.serial_baud_rate = 			param_find(baudRateParam);
-	_param_handles.address = 					param_find("RBCLW_ADDRESS");
-
-	_parameters_update();
-
-	// start serial port
-	_uart = open(deviceName, O_RDWR | O_NOCTTY);
-
-	if (_uart < 0) { err(1, "could not open %s", deviceName); }
-
-	int ret = 0;
-	struct termios uart_config {};
-	ret = tcgetattr(_uart, &uart_config);
-
-	if (ret < 0) { err(1, "failed to get attr"); }
-
-	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
-	ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate);
-
-	if (ret < 0) { err(1, "failed to set input speed"); }
-
-	ret = cfsetospeed(&uart_config, _parameters.serial_baud_rate);
-
-	if (ret < 0) { err(1, "failed to set output speed"); }
-
-	ret = tcsetattr(_uart, TCSANOW, &uart_config);
-
-	if (ret < 0) { err(1, "failed to set attr"); }
-
-	FD_ZERO(&_uart_set);
-
-	// setup default settings, reset encoders
-	resetEncoders();
-}
-
-RoboClaw::~RoboClaw()
-{
-	setMotorDutyCycle(MOTOR_1, 0.0);
-	setMotorDutyCycle(MOTOR_2, 0.0);
-	close(_uart);
-}
-
-void RoboClaw::taskMain()
-{
-	// Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not.
-	uint8_t rbuff[4];
-	int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true);
-
-	if (err_code <= 0) {
-		PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver.");
-		return;
-	}
-
-	// This main loop performs two different tasks, asynchronously:
-	// - Send actuator_controls_0 to the Roboclaw as soon as they are available
-	// - Read the encoder values at a constant rate
-	// To do this, the timeout on the poll() function is used.
-	// waitTime is the amount of time left (int microseconds) until the next time I should read from the encoders.
-	// It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before
-	// I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return
-	// immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment)
-	uint64_t encoderTaskLastRun = 0;
-	int waitTime = 0;
-
-	uint64_t actuatorsLastWritten = 0;
-
-	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
-	orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
-
-	_armedSub = orb_subscribe(ORB_ID(actuator_armed));
-	_paramSub = orb_subscribe(ORB_ID(parameter_update));
-
-	pollfd fds[3];
-	fds[0].fd = _paramSub;
-	fds[0].events = POLLIN;
-	fds[1].fd = _actuatorsSub;
-	fds[1].events = POLLIN;
-	fds[2].fd = _armedSub;
-	fds[2].events = POLLIN;
-
-	memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg));
-	_wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev;
-	_wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev;
-
-	while (!taskShouldExit) {
-
-		int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000);
-
-		bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms;
-
-		if (fds[0].revents & POLLIN) {
-			orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate);
-			_parameters_update();
-		}
-
-		// No timeout, update on either the actuator controls or the armed state
-		if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN || actuators_timeout)) {
-			orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls);
-			orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed);
-
-			int drive_ret = 0, turn_ret = 0;
-
-			const bool disarmed = !_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown
-					      || _actuatorArmed.force_failsafe || actuators_timeout;
-
-			if (disarmed) {
-				// If disarmed, I want to be certain that the stop command gets through.
-				int tries = 0;
-
-				while (tries < STOP_RETRIES && ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0)) {
-					PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret);
-					tries++;
-					px4_usleep(TIMEOUT_US);
-				}
-
-			} else {
-				drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]);
-				turn_ret = turn(_actuatorControls.control[actuator_controls_s::INDEX_YAW]);
-
-				if (drive_ret <= 0 || turn_ret <= 0) {
-					PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret);
-				}
-			}
-
-			actuatorsLastWritten = hrt_absolute_time();
-
-		} else {
-			// A timeout occurred, which means that it's time to update the encoders
-			encoderTaskLastRun = hrt_absolute_time();
-
-			if (readEncoder() > 0) {
-
-				for (int i = 0; i < 2; i++) {
-					_wheelEncoderMsg[i].timestamp = encoderTaskLastRun;
-
-					_wheelEncoderMsg[i].encoder_position = _encoderCounts[i];
-					_wheelEncoderMsg[i].speed = _motorSpeeds[i];
-
-					_wheelEncodersAdv[i].publish(_wheelEncoderMsg[i]);
-				}
-
-			} else {
-				PX4_ERR("Error reading encoders");
-			}
-		}
-
-		waitTime = _parameters.encoder_read_period_ms * 1000 - (hrt_absolute_time() - encoderTaskLastRun);
-		waitTime = waitTime < 0 ? 0 : waitTime;
-	}
-
-	orb_unsubscribe(_actuatorsSub);
-	orb_unsubscribe(_armedSub);
-	orb_unsubscribe(_paramSub);
-}
-
-int RoboClaw::readEncoder()
-{
-
-	uint8_t rbuff_pos[ENCODER_MESSAGE_SIZE];
-	// I am saving space by overlapping the two separate motor speeds, so that the final buffer will look like:
-	// [<speed 1> <speed 2> <status 2> <checksum 2>]
-	// And I just ignore all of the statuses and checksums. (The _transaction() function internally handles the
-	// checksum)
-	uint8_t rbuff_speed[ENCODER_SPEED_MESSAGE_SIZE + 4];
-
-	bool success = false;
-
-	for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) {
-		success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false,
-				       true) == ENCODER_MESSAGE_SIZE;
-		success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false,
-						  true) == ENCODER_SPEED_MESSAGE_SIZE;
-		success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false,
-						  true) == ENCODER_SPEED_MESSAGE_SIZE;
-	}
-
-	if (!success) {
-		PX4_ERR("Error reading encoders");
-		return -1;
-	}
-
-	uint32_t count;
-	uint32_t speed;
-	uint8_t *count_bytes;
-	uint8_t *speed_bytes;
-
-	for (int motor = 0; motor <= 1; motor++) {
-		count = 0;
-		speed = 0;
-		count_bytes = &rbuff_pos[motor * 4];
-		speed_bytes = &rbuff_speed[motor * 4];
-
-		// Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the
-		// endianness of the system this code is running on.
-		for (int byte = 0; byte < 4; byte++) {
-			count = (count << 8) + count_bytes[byte];
-			speed = (speed << 8) + speed_bytes[byte];
-		}
-
-		// The Roboclaw stores encoder counts as unsigned 32-bit ints. This can overflow, especially when starting
-		// at 0 and moving backward. The Roboclaw has overflow flags for this, but in my testing, they don't seem
-		// to work. This code detects overflow manually.
-		// These diffs are the difference between the count I just read from the Roboclaw and the last
-		// count that was read from the roboclaw for this motor. fwd_diff assumes that the wheel moved forward,
-		// and rev_diff assumes it moved backward. If the motor actually moved forward, then rev_diff will be close
-		// to 2^32 (UINT32_MAX). If the motor actually moved backward, then fwd_diff will be close to 2^32.
-		// To detect and account for overflow, I just assume that the smaller diff is correct.
-		// Strictly speaking, if the wheel rotated more than 2^31 encoder counts since the last time I checked, this
-		// will be wrong. But that's 1.7 million revolutions, so it probably won't come up.
-		uint32_t fwd_diff = count - _lastEncoderCount[motor];
-		uint32_t rev_diff = _lastEncoderCount[motor] - count;
-		// At this point, abs(diff) is always <= 2^31, so this cast from unsigned to signed is safe.
-		int32_t diff = fwd_diff <= rev_diff ? fwd_diff : -int32_t(rev_diff);
-		_encoderCounts[motor] += diff;
-		_lastEncoderCount[motor] = count;
-
-		_motorSpeeds[motor] = speed;
-	}
-
-	return 1;
-}
-
-void RoboClaw::printStatus(char *string, size_t n)
-{
-	snprintf(string, n, "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
-		 double(getMotorPosition(MOTOR_1)),
-		 double(getMotorSpeed(MOTOR_1)),
-		 double(getMotorPosition(MOTOR_2)),
-		 double(getMotorSpeed(MOTOR_2)));
-}
-
-float RoboClaw::getMotorPosition(e_motor motor)
-{
-	if (motor == MOTOR_1) {
-		return _encoderCounts[0];
-
-	} else if (motor == MOTOR_2) {
-		return _encoderCounts[1];
-
-	} else {
-		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
-		return NAN;
-	}
-}
-
-float RoboClaw::getMotorSpeed(e_motor motor)
-{
-	if (motor == MOTOR_1) {
-		return _motorSpeeds[0];
-
-	} else if (motor == MOTOR_2) {
-		return _motorSpeeds[1];
-
-	} else {
-		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
-		return NAN;
-	}
-}
-
-int RoboClaw::setMotorSpeed(e_motor motor, float value)
-{
-	e_command command;
-
-	// send command
-	if (motor == MOTOR_1) {
-		if (value > 0) {
-			command = CMD_DRIVE_FWD_1;
-
-		} else {
-			command = CMD_DRIVE_REV_1;
-		}
-
-	} else if (motor == MOTOR_2) {
-		if (value > 0) {
-			command = CMD_DRIVE_FWD_2;
-
-		} else {
-			command = CMD_DRIVE_REV_2;
-		}
-
-	} else {
-		return -1;
-	}
-
-	return _sendUnsigned7Bit(command, value);
-}
-
-int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
-{
-
-	e_command command;
-
-	// send command
-	if (motor == MOTOR_1) {
-		command = CMD_SIGNED_DUTYCYCLE_1;
-
-	} else if (motor == MOTOR_2) {
-		command = CMD_SIGNED_DUTYCYCLE_2;
-
-	} else {
-		return -1;
-	}
-
-	return _sendSigned16Bit(command, value);
-}
-
-int RoboClaw::drive(float value)
-{
-	e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX;
-	return _sendUnsigned7Bit(command, value);
-}
-
-int RoboClaw::turn(float value)
-{
-	e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT;
-	return _sendUnsigned7Bit(command, value);
-}
-
-int RoboClaw::resetEncoders()
-{
-	return _sendNothing(CMD_RESET_ENCODERS);
-}
-
-int RoboClaw::_sendUnsigned7Bit(e_command command, float data)
-{
-	data = fabs(data);
-
-	if (data > 1.0f) {
-		data = 1.0f;
-	}
-
-	auto byte = (uint8_t)(data * INT8_MAX);
-	uint8_t recv_byte;
-	return _transaction(command, &byte, 1, &recv_byte, 1);
-}
-
-int RoboClaw::_sendSigned16Bit(e_command command, float data)
-{
-	if (data > 1.0f) {
-		data = 1.0f;
-
-	} else if (data < -1.0f) {
-		data = -1.0f;
-	}
-
-	auto buff = (uint16_t)(data * INT16_MAX);
-	uint8_t recv_buff;
-	return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 1);
-}
-
-int RoboClaw::_sendNothing(e_command command)
-{
-	uint8_t recv_buff;
-	return _transaction(command, nullptr, 0, &recv_buff, 1);
-}
-
-uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init)
-{
-	uint16_t crc = init;
-
-	for (size_t byte = 0; byte < n; byte++) {
-		crc = crc ^ (((uint16_t) buf[byte]) << 8);
-
-		for (uint8_t bit = 0; bit < 8; bit++) {
-			if (crc & 0x8000) {
-				crc = (crc << 1) ^ 0x1021;
-
-			} else {
-				crc = crc << 1;
-			}
-		}
-	}
-
-	return crc;
-}
-
-int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
-			   uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum)
-{
-	int err_code = 0;
-
-	// WRITE
-
-	tcflush(_uart, TCIOFLUSH); // flush  buffers
-	uint8_t buf[wbytes + 4];
-	buf[0] = (uint8_t) _parameters.address;
-	buf[1] = cmd;
-
-	if (wbuff) {
-		memcpy(&buf[2], wbuff, wbytes);
-	}
-
-	wbytes = wbytes + (send_checksum ? 4 : 2);
-
-	if (send_checksum) {
-		uint16_t sum = _calcCRC(buf, wbytes - 2);
-		buf[wbytes - 2] = (sum >> 8) & 0xFF;
-		buf[wbytes - 1] = sum & 0xFFu;
-	}
-
-	int count = write(_uart, buf, wbytes);
-
-	if (count < (int) wbytes) { // Did not successfully send all bytes.
-		PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes);
-		return -1;
-	}
-
-	// READ
-
-	FD_ZERO(&_uart_set);
-	FD_SET(_uart, &_uart_set);
-
-	uint8_t *rbuff_curr = rbuff;
-	size_t bytes_read = 0;
-
-	// select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many
-	// bytes are available. I need to keep reading until I get the number of bytes I expect.
-	while (bytes_read < rbytes) {
-		// select(...) may change this timeout struct (because it is not const). So I reset it every time.
-		_uart_timeout.tv_sec = 0;
-		_uart_timeout.tv_usec = TIMEOUT_US;
-		err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout);
-
-		// An error code of 0 means that select timed out, which is how the Roboclaw indicates an error.
-		if (err_code <= 0) {
-			return err_code;
-		}
-
-		err_code = read(_uart, rbuff_curr, rbytes - bytes_read);
-
-		if (err_code <= 0) {
-			return err_code;
-
-		} else {
-			bytes_read += err_code;
-			rbuff_curr += err_code;
-		}
-	}
-
-	//TODO: Clean up this mess of IFs and returns
-
-	if (recv_checksum) {
-		if (bytes_read < 2) {
-			return -1;
-		}
-
-		// The checksum sent back by the roboclaw is calculated based on the address and command bytes as well
-		// as the data returned.
-		uint16_t checksum_calc = _calcCRC(buf, 2);
-		checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc);
-		uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1];
-
-		if (checksum_calc == checksum_recv) {
-			return bytes_read;
-
-		} else {
-			return -10;
-		}
-
-	} else {
-		if (bytes_read == 1 && rbuff[0] == 0xFF) {
-			return 1;
-
-		} else {
-			return -11;
-		}
-	}
-}
-
-void RoboClaw::_parameters_update()
-{
-	param_get(_param_handles.counts_per_rev, &_parameters.counts_per_rev);
-	param_get(_param_handles.encoder_read_period_ms, &_parameters.encoder_read_period_ms);
-	param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms);
-	param_get(_param_handles.address, &_parameters.address);
-
-	if (_actuatorsSub > 0) {
-		orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
-	}
-
-	int32_t baudRate;
-	param_get(_param_handles.serial_baud_rate, &baudRate);
-
-	switch (baudRate) {
-	case 2400:
-		_parameters.serial_baud_rate = B2400;
-		break;
-
-	case 9600:
-		_parameters.serial_baud_rate = B9600;
-		break;
-
-	case 19200:
-		_parameters.serial_baud_rate = B19200;
-		break;
-
-	case 38400:
-		_parameters.serial_baud_rate = B38400;
-		break;
-
-	case 57600:
-		_parameters.serial_baud_rate = B57600;
-		break;
-
-	case 115200:
-		_parameters.serial_baud_rate = B115200;
-		break;
-
-	case 230400:
-		_parameters.serial_baud_rate = B230400;
-		break;
-
-	case 460800:
-		_parameters.serial_baud_rate = B460800;
-		break;
-
-	default:
-		_parameters.serial_baud_rate = B2400;
-	}
-}
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 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 RoboClas.hpp
- *
- * RoboClaw Motor Driver
- *
- * references:
- * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
- *
- */
-
-#pragma once
-
-#include <poll.h>
-#include <stdio.h>
-#include <termios.h>
-#include <lib/parameters/param.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/wheel_encoders.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-#include <drivers/device/i2c.h>
-#include <sys/select.h>
-#include <sys/time.h>
-#include <pthread.h>
-
-/**
- * This is a driver for the RoboClaw motor controller
- */
-class RoboClaw
-{
-public:
-
-	void taskMain();
-	static bool taskShouldExit;
-
-	/** control channels */
-	enum e_channel {
-		CH_VOLTAGE_LEFT = 0,
-		CH_VOLTAGE_RIGHT
-	};
-
-	/**  motors */
-	enum e_motor {
-		MOTOR_1 = 0,
-		MOTOR_2
-	};
-
-	/**
-	 * constructor
-	 * @param deviceName the name of the
-	 * 	serial port e.g. "/dev/ttyS2"
-	 * @param address the adddress  of the motor
-	 * 	(selectable on roboclaw)
-	 * @param baudRateParam Name of the parameter that holds the baud rate of this serial port
-	 */
-	RoboClaw(const char *deviceName, const char *baudRateParam);
-
-	/**
-	 * deconstructor
-	 */
-	virtual ~RoboClaw();
-
-	/**
-	 * @return position of a motor, rev
-	 */
-	float getMotorPosition(e_motor motor);
-
-	/**
-	 * @return speed of a motor, rev/sec
-	 */
-	float getMotorSpeed(e_motor motor);
-
-	/**
-	 * set the speed of a motor, rev/sec
-	 */
-	int setMotorSpeed(e_motor motor, float value);
-
-	/**
-	 * set the duty cycle of a motor
-	 */
-	int setMotorDutyCycle(e_motor motor, float value);
-
-	/**
-	 * Drive both motors. +1 = full forward, -1 = full backward
-	 */
-	int drive(float value);
-
-	/**
-	 * Turn. +1 = full right, -1 = full left
-	 */
-	int turn(float value);
-
-	/**
-	 * reset the encoders
-	 * @return status
-	 */
-	int resetEncoders();
-
-	/**
-	 * read data from serial
-	 */
-	int readEncoder();
-
-	/**
-	 * print status
-	 */
-	void printStatus(char *string, size_t n);
-
-private:
-
-	// commands
-	// We just list the commands we want from the manual here.
-	enum e_command {
-
-		// simple
-		CMD_DRIVE_FWD_1 = 0,
-		CMD_DRIVE_REV_1 = 1,
-		CMD_DRIVE_FWD_2 = 4,
-		CMD_DRIVE_REV_2 = 5,
-
-		CMD_DRIVE_FWD_MIX = 8,
-		CMD_DRIVE_REV_MIX = 9,
-		CMD_TURN_RIGHT = 10,
-		CMD_TURN_LEFT = 11,
-
-		// encoder commands
-		CMD_READ_ENCODER_1 = 16,
-		CMD_READ_ENCODER_2 = 17,
-		CMD_READ_SPEED_1 = 18,
-		CMD_READ_SPEED_2 = 19,
-		CMD_RESET_ENCODERS = 20,
-		CMD_READ_BOTH_ENCODERS = 78,
-		CMD_READ_BOTH_SPEEDS = 79,
-
-		// advanced motor control
-		CMD_READ_SPEED_HIRES_1 = 30,
-		CMD_READ_SPEED_HIRES_2 = 31,
-		CMD_SIGNED_DUTYCYCLE_1 = 32,
-		CMD_SIGNED_DUTYCYCLE_2 = 33,
-
-		CMD_READ_STATUS = 90
-	};
-
-	struct {
-		speed_t serial_baud_rate;
-		int32_t counts_per_rev;
-		int32_t encoder_read_period_ms;
-		int32_t actuator_write_period_ms;
-		int32_t address;
-	} _parameters{};
-
-	struct {
-		param_t serial_baud_rate;
-		param_t counts_per_rev;
-		param_t encoder_read_period_ms;
-		param_t actuator_write_period_ms;
-		param_t address;
-	} _param_handles{};
-
-	int _uart;
-	fd_set _uart_set;
-	struct timeval _uart_timeout;
-
-	/** actuator controls subscription */
-	int _actuatorsSub{-1};
-	actuator_controls_s _actuatorControls;
-
-	int _armedSub{-1};
-	actuator_armed_s _actuatorArmed;
-
-	int _paramSub{-1};
-	parameter_update_s _paramUpdate;
-
-	uORB::PublicationMulti<wheel_encoders_s> _wheelEncodersAdv[2] { ORB_ID(wheel_encoders), ORB_ID(wheel_encoders)};
-	wheel_encoders_s _wheelEncoderMsg[2];
-
-	uint32_t _lastEncoderCount[2] {0, 0};
-	int64_t _encoderCounts[2] {0, 0};
-	int32_t _motorSpeeds[2] {0, 0};
-
-	void _parameters_update();
-
-	static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0);
-	int _sendUnsigned7Bit(e_command command, float data);
-	int _sendSigned16Bit(e_command command, float data);
-	int _sendNothing(e_command);
-
-	/**
-	 * Perform a round-trip write and read.
-	 *
-	 * NOTE: This function is not thread-safe.
-	 *
-	 * @param cmd Command to send to the Roboclaw
-	 * @param wbuff Write buffer. Must not contain command, address, or checksum. For most commands, this will be
-	 *   one or two bytes. Can be null iff wbytes == 0.
-	 * @param wbytes Number of bytes to write. Can be 0.
-	 * @param rbuff Read buffer. Will be filled with the entire response, including a checksum if the Roboclaw sends
-	 *   a checksum for this command.
-	 * @param rbytes Maximum number of bytes to read.
-	 * @param send_checksum If true, then the checksum will be calculated and sent to the Roboclaw.
-	 *   This is an option because some Roboclaw commands expect no checksum.
-	 * @param recv_checksum If true, then this function will calculate the checksum of the returned data and compare
-	 *   it to the checksum received. If they are not equal, OR if fewer than 2 bytes were received, then an
-	 *   error is returned.
-	 *   If false, then this function will expect to read exactly one byte, 0xFF, and will return an error otherwise.
-	 * @return If successful, then the number of bytes read from the Roboclaw is returned. If there is a timeout
-	 *   reading from the Roboclaw, then 0 is returned. If there is an IO error, then a negative value is returned.
-	 */
-	int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
-			 uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false);
-};
@@ -1,6 +0,0 @@
-module_name: Roboclaw Driver
-serial_config:
-  - command: roboclaw start ${SERIAL_DEV} ${BAUD_PARAM}
-    port_config_param:
-      name: RBCLW_SER_CFG
-      group: Roboclaw
@@ -1,207 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 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 roboclaw_main.cpp
- *
- * RoboClaw Motor Driver
- *
- * references:
- * http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf
- *
- */
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/log.h>
-#include <px4_platform_common/module.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <parameters/param.h>
-
-
-#include "RoboClaw.hpp"
-
-static bool thread_running = false;     /**< Deamon status flag */
-px4_task_t deamon_task;
-
-/**
- * Deamon management function.
- */
-extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]);
-
-/**
- * Mainloop of deamon.
- */
-int roboclaw_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage();
-
-static void usage()
-{
-	PRINT_MODULE_USAGE_NAME("roboclaw", "driver");
-
-	PRINT_MODULE_DESCRIPTION(R"DESCR_STR(
-### Description
-
-This driver communicates over UART with the [Roboclaw motor driver](http://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf).
-It performs two tasks:
-
- Control the motors based on the `actuator_controls_0` UOrb topic.
- Read the wheel encoders and publish the raw data in the `wheel_encoders` UOrb topic
-
-In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and
-your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. For Pixhawk 4,
-use the `UART & I2C B` port, which corresponds to `/dev/ttyS3`.
-
-### Implementation
-
-The main loop of this module (Located in `RoboClaw.cpp::task_main()`) performs 2 tasks:
-
- 1. Write `actuator_controls_0` messages to the Roboclaw as they become available
- 2. Read encoder data from the Roboclaw at a constant, fixed rate.
-
-Because of the latency of UART, this driver does not write every single `actuator_controls_0` message to the Roboclaw
-immediately. Instead, it is rate limited based on the parameter `RBCLW_WRITE_PER`.
-
-On startup, this driver will attempt to read the status of the Roboclaw to verify that it is connected. If this fails,
-the driver terminates immediately.
-
-### Examples
-
-The command to start this driver is:
-
-```
-$ roboclaw start <device> <baud>
-```
-
- `<device>` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`.
- `<baud>` is the baud rate.
-
-All available commands are:
-
- `$ roboclaw start <device> <baud>`
- `$ roboclaw status`
- `$ roboclaw stop`
-	)DESCR_STR");
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int roboclaw_main(int argc, char *argv[])
-{
-
-	if (argc < 4) {
-		usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("roboclaw already running\n");
-			/* this is not an error */
-			return 0;
-		}
-
-		RoboClaw::taskShouldExit = false;
-		deamon_task = px4_task_spawn_cmd("roboclaw",
-						 SCHED_DEFAULT,
-						 SCHED_PRIORITY_MAX - 10,
-						 2000,
-						 roboclaw_thread_main,
-						 (char *const *)argv);
-		return 0;
-
-	} else if (!strcmp(argv[1], "stop")) {
-
-		RoboClaw::taskShouldExit = true;
-		return 0;
-
-	} else if (!strcmp(argv[1], "status")) {
-
-		if (thread_running) {
-			printf("\troboclaw app is running\n");
-
-		} else {
-			printf("\troboclaw app not started\n");
-		}
-
-		return 0;
-	}
-
-	usage();
-	return 1;
-}
-
-int roboclaw_thread_main(int argc, char *argv[])
-{
-	printf("[roboclaw] starting\n");
-
-	// skip parent process args
-	argc -= 2;
-	argv += 2;
-
-	if (argc < 2) {
-		printf("usage: roboclaw start <device> <baud>\n");
-		return -1;
-	}
-
-	const char *deviceName = argv[1];
-	const char *baudRate = argv[2];
-
-	// start
-	RoboClaw roboclaw(deviceName, baudRate);
-
-	thread_running = true;
-
-	roboclaw.taskMain();
-
-	// exit
-	printf("[roboclaw] exiting.\n");
-	thread_running = false;
-	return 0;
-}
@@ -1,114 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2019 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 roboclaw_params.c
- *
- * Parameters defined by the Roboclaw driver.
- *
- * The Roboclaw will need to be configured to match these parameters. For information about configuring the
- * Roboclaw, see http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf
- *
- * @author Timothy Scott <timothy@auterion.com>
- */
-
-
-/**
- * Uart write period
- *
- * How long to wait, in Milliseconds, between writing actuator controls over Uart to the Roboclaw
- * @unit ms
- * @min 1
- * @max 1000
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_WRITE_PER, 10);
-
-/**
- * Encoder read period
- *
- * How long to wait, in Milliseconds, between reading wheel encoder values over Uart from the Roboclaw
- * @unit ms
- * @min 1
- * @max 1000
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_READ_PER, 10);
-
-/**
- * Encoder counts per revolution
- *
- * Number of encoder counts for one revolution. The roboclaw treats analog encoders (potentiometers) as having 2047
- * counts per rev. The default value of 1200 corresponds to the default configuration of the Aion R1 rover.
- * @min 1
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200);
-
-/**
- * Address of the Roboclaw
- *
- * The Roboclaw can be configured to have an address from 0x80 to 0x87, inclusive. It must be configured to match
- * this parameter.
- * @min 128
- * @max 135
- * @value 128 0x80
- * @value 129 0x81
- * @value 130 0x82
- * @value 131 0x83
- * @value 132 0x84
- * @value 133 0x85
- * @value 134 0x86
- * @value 135 0x87
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128);
-
-/**
- * Roboclaw serial baud rate
- *
- * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate.
- * @min 2400
- * @max 460800
- * @value 2400 2400 baud
- * @value 9600 9600 baud
- * @value 19200 19200 baud
- * @value 38400 38400 baud
- * @value 57600 57600 baud
- * @value 115200 115200 baud
- * @value 230400 230400 baud
- * @value 460800 460800 baud
- * @group Roboclaw driver
- * @reboot_required true
- */
-PARAM_DEFINE_INT32(RBCLW_BAUD, 2400);
@@ -46,38 +46,12 @@

 #include <float.h>

-using matrix::Vector2d;
 using matrix::Vector2f;
-using matrix::wrap_pi;
-
-void ECL_L1_Pos_Controller::update_roll_setpoint()
-{
-	float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
-	roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad);
-
-	if (_dt > 0.0f && _roll_slew_rate > 0.0f) {
-		// slew rate limiting active
-		roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt);
-	}
-
-	if (PX4_ISFINITE(roll_new)) {
-		_roll_setpoint = roll_new;
-	}
-
-}
-
-float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
-{
-	/* following [2], switching on L1 distance */
-	return math::min(wp_radius, _L1_distance);
-}

 void
 ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
 		const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
 {
-	_has_guidance_updated = true;
-
 	/* this follows the logic presented in [1] */
 	float eta = 0.0f;

@@ -202,181 +176,6 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
 	_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);

-	/* flying to waypoints, not circling them */
-	_circle_mode = false;
-
-	/* the bearing angle, in NED frame */
-	_bearing_error = eta;
-
-	update_roll_setpoint();
-}
-
-void
-ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
-				       const bool loiter_direction_counter_clockwise, const Vector2f &ground_speed_vector)
-{
-	_has_guidance_updated = true;
-
-	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
-
-	/* the complete guidance logic in this section was proposed by [2] */
-
-	/* calculate the gains for the PD loop (circle tracking) */
-	float omega = (2.0f * M_PI_F / _L1_period);
-	float K_crosstrack = omega * omega;
-	float K_velocity = 2.0f * _L1_damping * omega;
-
-	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
-	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
-
-	/* calculate the L1 length required for the desired period */
-	_L1_distance = _L1_ratio * ground_speed;
-
-	/* calculate the vector from waypoint A to current position */
-	Vector2f vector_A_to_airplane = vector_curr_position - vector_A;
-
-	Vector2f vector_A_to_airplane_unit;
-
-	/* prevent NaN when normalizing */
-	if (vector_A_to_airplane.length() > FLT_EPSILON) {
-		/* store the normalized vector from waypoint A to current position */
-		vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
-
-	} else {
-		vector_A_to_airplane_unit = vector_A_to_airplane;
-	}
-
-	/* update bearing to next waypoint */
-	_target_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
-
-	/* calculate eta angle towards the loiter center */
-
-	/* velocity across / orthogonal to line from waypoint to current position */
-	float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
-	/* velocity along line from waypoint to current position */
-	float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
-	float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
-	/* limit eta to 90 degrees */
-	eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
-
-	/* calculate the lateral acceleration to capture the center point */
-	float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
-
-	/* for PD control: Calculate radial position and velocity errors */
-
-	/* radial velocity error */
-	float xtrack_vel_circle = -ltrack_vel_center;
-	/* radial distance from the loiter circle (not center) */
-	float xtrack_err_circle = vector_A_to_airplane.length() - radius;
-
-	/* cross track error for feedback */
-	_crosstrack_error = xtrack_err_circle;
-
-	/* calculate PD update to circle waypoint */
-	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
-
-	/* calculate velocity on circle / along tangent */
-	float tangent_vel = xtrack_vel_center * loiter_direction_multiplier;
-
-	/* prevent PD output from turning the wrong way when in circle mode */
-	const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
-
-	if (tangent_vel < -l1_op_tan_vel && _circle_mode) {
-		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f);
-	}
-
-	/* calculate centripetal acceleration setpoint */
-	float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius),
-			(radius + xtrack_err_circle));
-
-	/* add PD control on circle and centripetal acceleration for total circle command */
-	float lateral_accel_sp_circle = loiter_direction_multiplier * (lateral_accel_sp_circle_pd +
-					lateral_accel_sp_circle_centripetal);
-
-	/*
-	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
-	 * the commands switch over. Only fly towards waypoint if outside the circle.
-	 */
-
-	// XXX check switch over
-	if ((lateral_accel_sp_center < lateral_accel_sp_circle && !loiter_direction_counter_clockwise
-	     && xtrack_err_circle > 0.0f)
-	    ||
-	    (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction_counter_clockwise && xtrack_err_circle > 0.0f)) {
-		_lateral_accel = lateral_accel_sp_center;
-		_circle_mode = false;
-		/* angle between requested and current velocity vector */
-		_bearing_error = eta;
-		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
-
-	} else {
-		_lateral_accel = lateral_accel_sp_circle;
-		_circle_mode = true;
-		_bearing_error = 0.0f;
-		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
-	}
-
-	update_roll_setpoint();
-}
-
-void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading,
-		const Vector2f &ground_speed_vector)
-{
-	_has_guidance_updated = true;
-
-	/* the complete guidance logic in this section was proposed by [2] */
-
-	/*
-	 * As the commanded heading is the only reference
-	 * (and no crosstrack correction occurs),
-	 * target and navigation bearing become the same
-	 */
-	_target_bearing = _nav_bearing = wrap_pi(navigation_heading);
-
-	float eta = wrap_pi(_target_bearing - wrap_pi(current_heading));
-
-	/* consequently the bearing error is exactly eta: */
-	_bearing_error = eta;
-
-	/* ground speed is the length of the ground speed vector */
-	float ground_speed = ground_speed_vector.length();
-
-	/* adjust L1 distance to keep constant frequency */
-	_L1_distance = ground_speed / _heading_omega;
-	float omega_vel = ground_speed * _heading_omega;
-
-	/* not circling a waypoint */
-	_circle_mode = false;
-
-	/* navigating heading means by definition no crosstrack error */
-	_crosstrack_error = 0;
-
-	/* limit eta to 90 degrees */
-	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
-	_lateral_accel = 2.0f * sinf(eta) * omega_vel;
-
-	update_roll_setpoint();
-}
-
-void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
-{
-	_has_guidance_updated = true;
-
-	/* the logic in this section is trivial, but originally proposed by [2] */
-
-	/* reset all heading / error measures resulting in zero roll */
-	_target_bearing = current_heading;
-	_nav_bearing = current_heading;
-	_bearing_error = 0;
-	_crosstrack_error = 0;
-	_lateral_accel = 0;
-
-	/* not circling a waypoint when flying level */
-	_circle_mode = false;
-
-	update_roll_setpoint();
 }

 void ECL_L1_Pos_Controller::set_l1_period(float period)
@@ -83,14 +83,6 @@ public:
 	 */
 	float nav_lateral_acceleration_demand() { return _lateral_accel; }

-	/**
-	 * Heading error.
-	 *
-	 * The heading error is either compared to the current track
-	 * or to the tangent of the current loiter radius.
-	 */
-	float bearing_error() { return _bearing_error; }
-
 	/**
 	 * Bearing from aircraft to current target.
 	 *
@@ -98,13 +90,6 @@ public:
 	 */
 	float target_bearing() { return _target_bearing; }

-	/**
-	 * Get roll angle setpoint for fixed wing.
-	 *
-	 * @return Roll angle (in NED frame)
-	 */
-	float get_roll_setpoint() { return _roll_setpoint; }
-
 	/**
 	 * Get the current crosstrack error.
 	 *
@@ -112,27 +97,6 @@ public:
 	 */
 	float crosstrack_error() { return _crosstrack_error; }

-	/**
-	 * Returns true if the loiter waypoint has been reached
-	 */
-	bool reached_loiter_target() { return _circle_mode; }
-
-	/**
-	 * Returns true if following a circle (loiter)
-	 */
-	bool circle_mode() { return _circle_mode; }
-
-	/**
-	 * Get the switch distance
-	 *
-	 * This is the distance at which the system will
-	 * switch to the next waypoint. This depends on the
-	 * period and damping
-	 *
-	 * @param waypoint_switch_radius The switching radius the waypoint has set.
-	 */
-	float switch_distance(float waypoint_switch_radius);
-
 	/**
 	 * Navigate between two waypoints
 	 *
@@ -145,35 +109,6 @@ public:
 	 */
 	void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
 				const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed);
-	/**
-	 * Navigate on an orbit around a loiter waypoint.
-	 *
-	 * This allow orbits smaller than the L1 length,
-	 * this modification was introduced in [2].
-	 *
-	 * @return sets _lateral_accel setpoint
-	 */
-	void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
-			     const bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_speed_vector);
-
-	/**
-	 * Navigate on a fixed bearing.
-	 *
-	 * This only holds a certain direction and does not perform cross
-	 * track correction. Helpful for semi-autonomous modes. Introduced
-	 * by [2].
-	 *
-	 * @return sets _lateral_accel setpoint
-	 */
-	void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed);
-
-	/**
-	 * Keep the wings level.
-	 *
-	 * This is typically needed for maximum-lift-demand situations,
-	 * such as takeoff or near stall. Introduced in [2].
-	 */
-	void navigate_level_flight(float current_heading);

 	/**
 	 * Set the L1 period.
@@ -187,32 +122,11 @@ public:
 	 */
 	void set_l1_damping(float damping);

-	/**
-	 * Set the maximum roll angle output in radians
-	 */
-	void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; }
-
-	/**
-	 * Set roll angle slew rate. Set to zero to deactivate.
-	 */
-	void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; }
-
-	/**
-	 * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
-	 */
-	void set_dt(float dt) { _dt = dt;}
-
-	void reset_has_guidance_updated() { _has_guidance_updated = false; }
-
-	bool has_guidance_updated() { return _has_guidance_updated; }
-
 private:

 	float _lateral_accel{0.0f};		///< Lateral acceleration setpoint in m/s^2
 	float _L1_distance{20.0f};		///< L1 lead distance, defined by period and damping
-	bool _circle_mode{false};		///< flag for loiter mode
 	float _nav_bearing{0.0f};		///< bearing to L1 reference point
-	float _bearing_error{0.0f};		///< bearing error
 	float _crosstrack_error{0.0f};	///< crosstrack error in meters
 	float _target_bearing{0.0f};		///< the heading setpoint

@@ -221,21 +135,6 @@ private:
 	float _L1_ratio{5.0f};		///< L1 ratio for navigation
 	float _K_L1{2.0f};			///< L1 control gain for _L1_damping
 	float _heading_omega{1.0f};		///< Normalized frequency
-
-	float _roll_lim_rad{math::radians(30.0f)};  ///<maximum roll angle in radians
-	float _roll_setpoint{0.0f};	///< current roll angle setpoint in radians
-	float _roll_slew_rate{0.0f};	///< roll angle setpoint slew rate limit in rad/s
-	float _dt{0};				///< control loop time in seconds
-
-	bool _has_guidance_updated =
-		false;	///< this flag is set to true by any of the guidance methods. This flag has to be manually reset using has_guidance_updated_reset()
-
-	/**
-	 * Update roll angle setpoint. This will also apply slew rate limits if set.
-	 *
-	 */
-	void update_roll_setpoint();
-
 };


@@ -583,7 +583,7 @@ public:

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

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

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

 		for (size_t i = 0; i < M; i++) {
 			for (size_t j = 0; j < N; j++) {
@@ -288,7 +288,7 @@ TECSControl::STERateLimit TECSControl::_calculateTotalEnergyRateLimit(const Para
 	TECSControl::STERateLimit limit;
 	// Calculate the specific total energy rate limits from the max throttle limits
 	limit.STE_rate_max = math::max(param.max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
-	limit.STE_rate_min = - math::max(param.max_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
+	limit.STE_rate_min = - math::max(param.min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 	// airspeed scale validity check
 	static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°)
--- a/Show More
+++ b/Show More