cmake: move FindPythonInterp->FindPython

- cmake CMP0148 FindPythonInterp deprecated
2026-05-27 23:10:05 +08:00 · 2024-11-22 21:57:53 -05:00
133 changed files with 1593 additions and 2828 deletions
@@ -25,14 +25,8 @@ jobs:
          submodules: false
          fetch-depth: 0

-      - name: Set PX4 Tag
-        id: px4-tag
-        run: |
-          echo "tag=$(git describe --tags --match 'v[0-9]*')" >> $GITHUB_OUTPUT
-
      - name: Login to Docker Hub
        uses: docker/login-action@v3
-        if: github.event_name != 'pull_request'
        with:
          username: ${{ secrets.DOCKERHUB_USERNAME }}
          password: ${{ secrets.DOCKERHUB_TOKEN }}
@@ -50,15 +44,16 @@ jobs:
        with:
          images: |
            ghcr.io/PX4/px4-dev
-            ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }}
+            px4io/px4-dev
          tags: |
+            type=schedule
            type=semver,pattern={{version}}
            type=semver,pattern={{major}}.{{minor}}
            type=semver,pattern={{major}}
-            type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700
-            type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600
+            type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}},priority=600
            type=ref,event=branch,suffix=,priority=500
            type=ref,event=pr
+            type=sha

      - name: Set up Docker Buildx
        uses: docker/setup-buildx-action@v3
@@ -173,9 +173,10 @@ set(config_module_list)
 set(config_kernel_list)

 # Find Python
-find_package(PythonInterp 3)
+find_package(Python COMPONENTS Interpreter)
+
 # We have a custom error message to tell users how to install python3.
-if(NOT PYTHONINTERP_FOUND)
+if(NOT Python_FOUND)
 	message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n"
 		"    Ubuntu: sudo apt install python3 python3-dev python3-pip\n"
 		"    macOS: brew install python")
@@ -184,7 +185,7 @@ endif()
 option(PYTHON_COVERAGE "Python code coverage" OFF)
 if(PYTHON_COVERAGE)
 	message(STATUS "python coverage enabled")
-	set(PYTHON_EXECUTABLE coverage run -p)
+	set(Python_EXECUTABLE coverage run -p)
 endif()

 include(px4_config)
@@ -481,7 +482,7 @@ foreach(module ${config_module_list})
 endforeach()

 add_custom_command(OUTPUT ${uorb_graph_config}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/uorb_graph/create.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/uorb_graph/create.py
 		${graph_module_list} --src-path src/lib
 		--merge-depends
 		--exclude-path src/examples
@@ -502,7 +503,7 @@ include(package)

 # install python requirements using configured python
 add_custom_target(install_python_requirements
-	COMMAND ${PYTHON_EXECUTABLE} -m pip install --break-system-packages --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
+	COMMAND ${Python_EXECUTABLE} -m pip install --break-system-packages --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
 	DEPENDS ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
 	USES_TERMINAL
 )
@@ -169,7 +169,7 @@ endif

 # Pick up specific Python path if set
 ifdef PYTHON_EXECUTABLE
-	override CMAKE_ARGS += -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}
+	override CMAKE_ARGS += -DPython_EXECUTABLE=${PYTHON_EXECUTABLE}
 endif

 # Functions
@@ -2,7 +2,7 @@

 [![Releases](https://img.shields.io/github/release/PX4/PX4-Autopilot.svg)](https://github.com/PX4/PX4-Autopilot/releases) [![DOI](https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot)

-[![Build Targets](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml/badge.svg?branch=main)](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)
+[![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)

 [![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode)

@@ -124,15 +124,15 @@ add_custom_command(
 		${romfs_copy_stamp}
 	COMMAND ${CMAKE_COMMAND} -E remove_directory ${romfs_gen_root_dir}/*
 	COMMAND ${CMAKE_COMMAND} -E tar xf ${romfs_tar_file}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
 		--airframes-path ${romfs_gen_root_dir}/init.d
 		--start-script ${romfs_gen_root_dir}/init.d/rc.autostart
 		--board ${PX4_BOARD}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
 		--rc-dir ${romfs_gen_root_dir}/init.d
 		--serial-ports ${board_serial_ports} ${added_arguments}
 		--config-files ${module_config_files} #--verbose
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/filepaths/generate_config.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/filepaths/generate_config.py
 		--rc-dir ${romfs_gen_root_dir}/init.d
 		--params-file ${CONFIG_BOARD_PARAM_FILE}
 	COMMAND ${CMAKE_COMMAND} -E touch ${romfs_copy_stamp}
@@ -330,7 +330,7 @@ add_custom_target(romfs_gen_files_target

 add_custom_command(
 	OUTPUT romfs_pruned.stamp
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_romfs_pruner.py --folder ${romfs_gen_root_dir} --board ${PX4_BOARD}
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_romfs_pruner.py --folder ${romfs_gen_root_dir} --board ${PX4_BOARD}
 	COMMAND ${CMAKE_COMMAND} -E touch romfs_pruned.stamp
 	DEPENDS
 		romfs_copy.stamp
@@ -38,6 +38,4 @@ param set-default PWM_MAIN_FUNC2 102
 param set-default PWM_MAIN_FUNC3 103
 param set-default PWM_MAIN_FUNC4 104

-param set-default EKF2_GPS_DELAY 0
-
 param set SIH_VEHICLE_TYPE 0
@@ -45,9 +45,7 @@ param set-default CA_SV_CS1_TRQ_P 1
 param set-default CA_SV_CS1_TYPE 3
 param set-default CA_SV_CS2_TRQ_Y 1
 param set-default CA_SV_CS2_TYPE 4
-param set-default PWM_MAIN_FUNC1 201
-param set-default PWM_MAIN_FUNC2 202
-param set-default PWM_MAIN_FUNC3 203
-param set-default PWM_MAIN_FUNC4 101
-
-param set-default EKF2_GPS_DELAY 0
+param set-default PWM_MAIN_FUNC3 201
+param set-default PWM_MAIN_FUNC4 202
+param set-default PWM_MAIN_FUNC5 203
+param set-default PWM_MAIN_FUNC6 101
@@ -11,7 +11,6 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}

-param set-default EKF2_GPS_DELAY 0
 param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters

 param set-default SENS_EN_GPSSIM 1
@@ -26,6 +26,7 @@ param set-default RD_MAX_THR_SPD 2.15
 param set-default RD_SPEED_P 0.1
 param set-default RD_SPEED_I 0.01
 param set-default RD_MAX_YAW_RATE 180
+param set-default RD_MISS_SPD_DEF 2
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 param set-default RD_MAX_YAW_ACCEL 1000
@@ -29,6 +29,7 @@ param set-default RD_MAX_SPEED 8
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0.1
 param set-default RD_MAX_YAW_RATE 30
+param set-default RD_MISS_SPD_DEF 8
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533

@@ -13,18 +13,18 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge

 # Rover parameters
 param set-default RM_WHEEL_TRACK 0.3
-param set-default RM_YAW_RATE_I 0.1
-param set-default RM_YAW_RATE_P 0.1
+param set-default RM_MAN_YAW_SCALE 0.1
+param set-default RM_YAW_RATE_I 0
+param set-default RM_YAW_RATE_P 0.01
 param set-default RM_MAX_ACCEL 3
-param set-default RM_MAX_DECEL 5
 param set-default RM_MAX_JERK 5
-param set-default RM_MAX_SPEED 2
-param set-default RM_MAX_THR_SPD 2.2
-param set-default RM_MAX_THR_YAW_R 1.2
+param set-default RM_MAX_SPEED 4
+param set-default RM_MAX_THR_SPD 7
+param set-default RM_MAX_THR_YAW_R 7.5
 param set-default RM_YAW_P 5
 param set-default RM_YAW_I 0.1
-param set-default RM_MAX_YAW_RATE 120
-param set-default RM_MAX_YAW_ACCEL 240
+param set-default RM_MAX_YAW_RATE 180
+param set-default RM_MISS_SPD_DEF 3
 param set-default RM_MISS_VEL_GAIN 1
 param set-default RM_SPEED_I 0.01
 param set-default RM_SPEED_P 0.1
@@ -42,23 +42,23 @@ param set-default SENS_EN_ARSPDSIM 0

 # Actuator mapping
 param set-default SIM_GZ_WH_FUNC1 102 # right wheel front
-param set-default SIM_GZ_WH_MIN1 70
-param set-default SIM_GZ_WH_MAX1 130
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
 param set-default SIM_GZ_WH_DIS1 100

 param set-default SIM_GZ_WH_FUNC2 101 # left wheel front
-param set-default SIM_GZ_WH_MIN2 70
-param set-default SIM_GZ_WH_MAX2 130
+param set-default SIM_GZ_WH_MIN2 0
+param set-default SIM_GZ_WH_MAX2 200
 param set-default SIM_GZ_WH_DIS2 100

 param set-default SIM_GZ_WH_FUNC3 104 # right wheel back
-param set-default SIM_GZ_WH_MIN3 70
-param set-default SIM_GZ_WH_MAX3 130
+param set-default SIM_GZ_WH_MIN3 0
+param set-default SIM_GZ_WH_MAX3 200
 param set-default SIM_GZ_WH_DIS3 100

 param set-default SIM_GZ_WH_FUNC4 103 # left wheel back
-param set-default SIM_GZ_WH_MIN4 70
-param set-default SIM_GZ_WH_MAX4 130
+param set-default SIM_GZ_WH_MIN4 0
+param set-default SIM_GZ_WH_MAX4 200
 param set-default SIM_GZ_WH_DIS4 100

 param set-default SIM_GZ_WH_REV 10
@@ -1,97 +0,0 @@
-#!/bin/sh
-#
-# @name Quadrotor + Tailsitter
-#
-# @type VTOL Quad Tailsitter
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
-PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
-PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter}
-
-param set-default SIM_GZ_EN 1 # Gazebo bridge
-
-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 0
-
-
-param set-default MAV_TYPE 20
-
-param set-default CA_AIRFRAME 4
-
-param set-default CA_ROTOR_COUNT 4
-param set-default CA_ROTOR0_PX 0.15
-param set-default CA_ROTOR0_PY 0.23
-param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR1_PX -0.15
-param set-default CA_ROTOR1_PY -0.23
-param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR2_PX 0.15
-param set-default CA_ROTOR2_PY -0.23
-param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.15
-param set-default CA_ROTOR3_PY 0.23
-param set-default CA_ROTOR3_KM -0.05
-
-param set-default CA_SV_CS_COUNT 0
-
-param set-default SIM_GZ_EC_FUNC1 101
-param set-default SIM_GZ_EC_FUNC2 102
-param set-default SIM_GZ_EC_FUNC3 103
-param set-default SIM_GZ_EC_FUNC4 104
-
-param set-default SIM_GZ_EC_MIN1 10
-param set-default SIM_GZ_EC_MIN2 10
-param set-default SIM_GZ_EC_MIN3 10
-param set-default SIM_GZ_EC_MIN4 10
-
-param set-default SIM_GZ_EC_MAX1 1500
-param set-default SIM_GZ_EC_MAX2 1500
-param set-default SIM_GZ_EC_MAX3 1500
-param set-default SIM_GZ_EC_MAX4 1500
-
-param set-default FD_FAIL_R 70
-
-param set-default FW_P_TC 0.6
-
-param set-default FW_PR_I 0.3
-param set-default FW_PR_P 0.5
-param set-default FW_PSP_OFF 2
-param set-default FW_RR_FF 0.1
-param set-default FW_RR_I 0.1
-param set-default FW_RR_P 0.2
-param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P
-param set-default FW_YR_I 0
-param set-default FW_THR_TRIM 0.35
-param set-default FW_THR_MAX 0.8
-param set-default FW_THR_MIN 0.05
-param set-default FW_T_CLMB_MAX 6
-param set-default FW_T_HRATE_FF 0.5
-param set-default FW_T_SINK_MAX 3
-param set-default FW_T_SINK_MIN 1.6
-param set-default FW_AIRSPD_STALL 10
-param set-default FW_AIRSPD_MIN 14
-param set-default FW_AIRSPD_TRIM 18
-param set-default FW_AIRSPD_MAX 22
-
-param set-default MC_AIRMODE 2
-param set-default MAN_ARM_GESTURE 0 # required for yaw airmode
-param set-default MC_ROLL_P 3
-param set-default MC_PITCH_P 3
-param set-default MC_ROLLRATE_P 0.3
-param set-default MC_PITCHRATE_P 0.3
-
-param set-default VT_ARSP_TRANS 15
-param set-default VT_B_TRANS_DUR 5
-param set-default VT_FW_DIFTHR_EN 7
-param set-default VT_FW_DIFTHR_S_Y 1
-param set-default VT_F_TRANS_DUR 1.5
-param set-default VT_TYPE 0
-
-param set-default WV_EN 0
-
-param set-default EKF2_FUSE_BETA 0
@@ -89,7 +89,6 @@ px4_add_romfs_files(
 	4015_gz_r1_rover_mecanum
 	4016_gz_x500_lidar_down
 	4017_gz_x500_lidar_front
-	4018_gz_quadtailsitter

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -18,19 +18,22 @@ param set UAVCAN_ENABLE 0
 param set-default CA_AIRFRAME 1
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
-param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_CS0_TRQ_R -0.5
-param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
 param set-default CA_SV_CS1_TRQ_P 1
 param set-default CA_SV_CS1_TYPE 3
 param set-default CA_SV_CS2_TRQ_Y 1
 param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS3_TYPE 10

-param set HIL_ACT_REV 1
-param set HIL_ACT_FUNC1 201
-param set HIL_ACT_FUNC2 202
-param set HIL_ACT_FUNC3 203
-param set HIL_ACT_FUNC4 101
+param set-default HIL_ACT_REV 2
+param set-default HIL_ACT_FUNC1 201
+param set-default HIL_ACT_FUNC2 202
+param set-default HIL_ACT_FUNC3 203
+param set-default HIL_ACT_FUNC4 101
+param set-default HIL_ACT_FUNC5 204
+param set-default HIL_ACT_FUNC6 400

 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set-default SYS_HITL 2
@@ -31,6 +31,7 @@ param set-default RD_MAX_THR_SPD 1.9
 param set-default RD_MAX_THR_YAW_R 0.7
 param set-default RD_MAX_YAW_ACCEL 600
 param set-default RD_MAX_YAW_RATE 250
+param set-default RD_MISS_SPD_DEF 1.5
 param set-default RD_SPEED_P 0.1
 param set-default RD_SPEED_I 0.01
 param set-default RD_TRANS_DRV_TRN 0.785398
@@ -13,6 +13,8 @@ param set-default MAV_TYPE 1
 #
 # Default parameters for fixed wing UAVs.
 #
+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_VEL_FS_EVH 3
@@ -211,13 +211,6 @@ then
 	spl06 -X -a 0x77 start
 fi

-# SPA06 sensor external I2C
-if param compare -s SENS_EN_SPA06 1
-then
-	spa06 -X start
-	spa06 -X -a 0x77 start
-fi
-
 # PCF8583 counter (RPM sensor)
 if param compare -s SENS_EN_PCF8583 1
 then
@@ -43,7 +43,7 @@ fi

 # install git pre-commit hook
 HOOK_FILE="$DIR/../../.git/hooks/pre-commit"
-if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ] && [ $- == *i* ]; then
+if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then
 	echo ""
 	echo -e "\033[31mNinja tip: add a git pre-commit hook to automatically check code style\033[0m"
 	echo -e "Would you like to install one now? (\033[94mcp ./Tools/astyle/pre-commit .git/hooks/pre-commit\033[0m): [y/\033[1mN\033[0m]"
@@ -137,7 +137,7 @@ div.frame_variant td, div.frame_variant th {
                            #print(output_name,value, attribstrs[0].strip(),attribstrs[1].strip())
                    outputs += '</ul>'
                    if has_outputs:
-                        outputs_entry = '<br><b>Specific Outputs:</b>' + outputs
+                        outputs_entry = '<p><b>Specific Outputs:</b>' + outputs + '</p>'
                    else:
                        outputs_entry = ''

@@ -1 +0,0 @@
-CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
@@ -11,7 +11,4 @@ then
 	icm42688p -R 6 -s start
 fi

-if ! bmp280 -X start
-then
-	spa06 -X start
-fi
+bmp280 -X start
@@ -12,7 +12,4 @@ then
  fi
 fi

-if ! bmp280 -X start
-then
-	spa06 -X start
-fi
+bmp280 -X start
@@ -9,7 +9,4 @@ then
 	icm42688p -R 0 -s start
 fi

-if ! bmp280 -X start
-then
-	spa06 -X start
-fi
+bmp280 -X start
@@ -67,7 +67,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
@@ -35,7 +35,7 @@
 set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.hex)

 add_custom_target(upload_teensy
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/teensy_uploader.py --port ${serial_ports} ${PX4_FW_NAME} --vendor-id 0x1FC9 --product-id 0x0024
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/teensy_uploader.py --port ${serial_ports} ${PX4_FW_NAME} --vendor-id 0x1FC9 --product-id 0x0024
 	DEPENDS ${PX4_FW_NAME}
 	COMMENT "uploading px4"
 	VERBATIM
@@ -68,7 +68,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
@@ -1,20 +1,20 @@
 set(BOARD_DEFCONFIG ${PX4_CONFIG_FILE} CACHE FILEPATH "path to defconfig" FORCE)
 set(BOARD_CONFIG ${PX4_BINARY_DIR}/boardconfig CACHE FILEPATH "path to config" FORCE)

-execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import menuconfig" RESULT_VARIABLE ret)
+execute_process(COMMAND ${Python_EXECUTABLE} -c "import menuconfig" RESULT_VARIABLE ret)
 if(ret EQUAL "1")
 	message(FATAL_ERROR "kconfiglib is not installed or not in PATH\n"
 				"please install using \"pip3 install kconfiglib\"\n")
 endif()

-set(MENUCONFIG_PATH ${PYTHON_EXECUTABLE} -m menuconfig CACHE INTERNAL "menuconfig program" FORCE)
-set(GUICONFIG_PATH ${PYTHON_EXECUTABLE} -m guiconfig CACHE INTERNAL "guiconfig program" FORCE)
-set(DEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m defconfig CACHE INTERNAL "defconfig program" FORCE)
-set(SAVEDEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m savedefconfig CACHE INTERNAL "savedefconfig program" FORCE)
-set(GENCONFIG_PATH ${PYTHON_EXECUTABLE} -m genconfig CACHE INTERNAL "genconfig program" FORCE)
+set(MENUCONFIG_PATH ${Python_EXECUTABLE} -m menuconfig CACHE INTERNAL "menuconfig program" FORCE)
+set(GUICONFIG_PATH ${Python_EXECUTABLE} -m guiconfig CACHE INTERNAL "guiconfig program" FORCE)
+set(DEFCONFIG_PATH ${Python_EXECUTABLE} -m defconfig CACHE INTERNAL "defconfig program" FORCE)
+set(SAVEDEFCONFIG_PATH ${Python_EXECUTABLE} -m savedefconfig CACHE INTERNAL "savedefconfig program" FORCE)
+set(GENCONFIG_PATH ${Python_EXECUTABLE} -m genconfig CACHE INTERNAL "genconfig program" FORCE)

 set(COMMON_KCONFIG_ENV_SETTINGS
-	PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}
+	Python_EXECUTABLE=${Python_EXECUTABLE}
 	KCONFIG_CONFIG=${BOARD_CONFIG}
 	# Set environment variables so that Kconfig can prune Kconfig source
 	# files for other architectures
@@ -47,7 +47,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
 		# Generate boardconfig from default.px4board and {label}.px4board
 		execute_process(
 			COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
-			${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/merge_config.py Kconfig ${BOARD_CONFIG} ${PX4_BOARD_DIR}/default.px4board ${BOARD_DEFCONFIG}
+			${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/merge_config.py Kconfig ${BOARD_CONFIG} ${PX4_BOARD_DIR}/default.px4board ${BOARD_DEFCONFIG}
 			WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 			OUTPUT_VARIABLE DUMMY_RESULTS
 		)
@@ -57,7 +57,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
 		message(AUTHOR_WARNING "allyes build: allyes is for CI coverage and not for use in production")
 		execute_process(
 			COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
-			${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/allyesconfig.py
+			${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/allyesconfig.py
 			WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 		)
 	endif()
@@ -452,7 +452,7 @@ elseif(NOT ${LABEL} MATCHES "allyes") # All other configs except allyes which is
 	add_custom_target(boardconfig
 		${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${MENUCONFIG_PATH} Kconfig
 		COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH}
-		COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
+		COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
 		COMMAND ${CMAKE_COMMAND} -E remove defconfig
 		COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp
 		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
@@ -463,7 +463,7 @@ elseif(NOT ${LABEL} MATCHES "allyes") # All other configs except allyes which is
 	add_custom_target(boardguiconfig
 		${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${GUICONFIG_PATH} Kconfig
 		COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH}
-		COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
+		COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
 		COMMAND ${CMAKE_COMMAND} -E remove defconfig
 		COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp
 		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
@@ -473,7 +473,7 @@ elseif(NOT ${LABEL} MATCHES "allyes") # All other configs except allyes which is

 	add_custom_target(px4_savedefconfig
 		${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH}
-		COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
+		COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
 		COMMAND ${CMAKE_COMMAND} -E remove defconfig
 		COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp
 		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
@@ -35,10 +35,10 @@

 add_custom_target(metadata_airframes
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
 		-v -a ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d
 		--markdown ${PX4_BINARY_DIR}/docs/airframes.md
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
 		-v -a ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d
 		--xml ${PX4_BINARY_DIR}/docs/airframes.xml
 	COMMENT "Generating full airframe metadata (markdown and xml)"
@@ -58,28 +58,28 @@ add_custom_target(metadata_parameters
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_params_dir}

-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
 		--all-ports --ethernet --params-file ${generated_params_dir}/serial_params.c --config-files ${yaml_config_files}

-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py
 		--params-file ${generated_params_dir}/module_params.c
 		--timer-config ${PX4_SOURCE_DIR}/boards/px4/fmu-v5/src/timer_config.cpp # select a typical board
 		--board-with-io
 		--ethernet
 		--config-files ${yaml_config_files} #--verbose

-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
 		--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir}
 		--inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml
 		--markdown ${PX4_BINARY_DIR}/docs/parameters.md

-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
 		--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir}
 		--inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml
 		--json ${PX4_BINARY_DIR}/docs/parameters.json
 		--compress

-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
 		--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir}
 		--inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml
 		--xml ${PX4_BINARY_DIR}/docs/parameters.xml
@@ -90,7 +90,7 @@ add_custom_target(metadata_parameters

 add_custom_target(metadata_module_documentation
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_module_doc.py -v --src-path ${PX4_SOURCE_DIR}/src
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_module_doc.py -v --src-path ${PX4_SOURCE_DIR}/src
 		--markdown ${PX4_BINARY_DIR}/docs/modules
 	COMMENT "Generating module documentation"
 	USES_TERMINAL
@@ -99,17 +99,17 @@ add_custom_target(metadata_module_documentation
 set(events_src_path "${PX4_SOURCE_DIR}/src/lib/events")
 add_custom_target(metadata_extract_events
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/events
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py
 		--src-path ${PX4_SOURCE_DIR}/src
 		--json ${PX4_BINARY_DIR}/events/px4_full.json #--verbose
-	COMMAND ${PYTHON_EXECUTABLE} ${events_src_path}/libevents/scripts/combine.py
+	COMMAND ${Python_EXECUTABLE} ${events_src_path}/libevents/scripts/combine.py
 		${PX4_BINARY_DIR}/events/px4_full.json
 		${events_src_path}/libevents/events/common.json
 		${events_src_path}/enums.json
 		--output ${PX4_BINARY_DIR}/events/all_events_full.json
-	COMMAND ${PYTHON_EXECUTABLE} ${events_src_path}/libevents/scripts/validate.py
+	COMMAND ${Python_EXECUTABLE} ${events_src_path}/libevents/scripts/validate.py
 		${PX4_BINARY_DIR}/events/all_events_full.json
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py
 		${PX4_BINARY_DIR}/events/all_events_full.json
 	COMMENT "Extracting events from full source"
 	USES_TERMINAL
@@ -67,7 +67,7 @@ function(px4_generate_airframes_xml)
 		ARGN ${ARGN})

 	add_custom_command(OUTPUT ${PX4_BINARY_DIR}/airframes.xml
-		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
+		COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
 			--airframes-path ${PX4_SOURCE_DIR}/ROMFS/${config_romfs_root}/init.d
 			--board CONFIG_ARCH_BOARD_${PX4_BOARD}
 			--xml ${PX4_BINARY_DIR}/airframes.xml
@@ -312,7 +312,7 @@ add_custom_command(
 	OUTPUT
 		${uorb_headers}
 		${msg_out_path}/uORBTopics.hpp
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
 		--headers
 		-f ${msg_files}
 		-i ${CMAKE_CURRENT_SOURCE_DIR}
@@ -333,7 +333,7 @@ add_custom_target(uorb_headers DEPENDS ${uorb_headers})
 add_custom_command(
 	OUTPUT
 		${uorb_json_files}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
 		--json
 		-f ${msg_files}
 		-i ${CMAKE_CURRENT_SOURCE_DIR}
@@ -356,7 +356,7 @@ add_custom_command(
 	OUTPUT
 		${uorb_message_fields_cpp_file}
 		${uorb_message_fields_header_file}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_compressed_fields.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_compressed_fields.py
 		-f ${uorb_json_files}
 		--source-output-file ${uorb_message_fields_cpp_file}
 		--header-output-file ${uorb_message_fields_header_file}
@@ -371,7 +371,7 @@ add_custom_command(
 # Generate microcdr headers
 add_custom_command(
 	OUTPUT ${uorb_ucdr_headers}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
 		--headers
 		-f ${msg_files}
 		-i ${CMAKE_CURRENT_SOURCE_DIR}
@@ -393,7 +393,7 @@ add_custom_command(
 	OUTPUT
 		${uorb_sources}
 		${msg_source_out_path}/uORBTopics.cpp
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
 		--sources
 		-f ${msg_files}
 		-i ${CMAKE_CURRENT_SOURCE_DIR}
@@ -459,7 +459,7 @@ if(CONFIG_LIB_CDRSTREAM)
 		OUTPUT ${uorb_cdr_idl}
 		COMMAND ${CMAKE_COMMAND}
 		        -E env "PYTHONPATH=${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_adapter:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_cli"
-			${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/msg2idl.py
+			${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/msg2idl.py
 			${uorb_cdr_msg}
 		DEPENDS
 			${uorb_cdr_msg}
@@ -489,7 +489,7 @@ if(CONFIG_LIB_CDRSTREAM)

 	add_custom_target(
 		uorb_idl_header
-		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+		COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
 			--uorb-idl-header
 			-f ${msg_files}
 			-i ${CMAKE_CURRENT_SOURCE_DIR}
@@ -513,7 +513,7 @@ endif()

 if(CONFIG_MODULES_ZENOH)
 	# Update kconfig file for topics
-	execute_process(COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
+	execute_process(COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
 			--zenoh-config
 			-f ${msg_files}
 			-o ${PX4_SOURCE_DIR}/src/modules/zenoh/
@@ -522,7 +522,7 @@ if(CONFIG_MODULES_ZENOH)
 	add_custom_command(
 		OUTPUT
 			${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp
-		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
+		COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
 			--zenoh-pub-sub
 			-f ${msg_files}
 			-o ${PX4_BINARY_DIR}/src/modules/zenoh/
@@ -5,7 +5,7 @@ float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward s
 float32 lateral_speed_setpoint            # [m/s] Desired lateral speed
 float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed
 float32 yaw_rate_setpoint                 # [rad/s] Desired yaw rate
-float32 speed_diff_setpoint_normalized    # [-1, 1] Normalized speed difference between the left and right wheels
+float32 yaw_rate_setpoint_normalized      # [-1, 1] Desired normalized yaw rate
 float32 yaw_setpoint 	                  # [rad] Desired yaw (heading)

 # TOPICS rover_mecanum_setpoint
@@ -1,17 +1,13 @@
 uint64 timestamp # time since system start (microseconds)

-float32 measured_forward_speed          # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
-float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate
-float32 measured_lateral_speed          # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
-float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate
-float32 measured_yaw_rate  	        # [rad/s] Measured yaw rate
-float32 clyaw_yaw_rate_setpoint         # [rad/s] Yaw rate setpoint output by the closed loop yaw controller
-float32 adjusted_yaw_rate_setpoint      # [rad/s] Yaw rate setpoint from the closed loop yaw controller
-float32 measured_yaw        	        # [rad] Measured yaw
-float32 adjusted_yaw_setpoint           # [rad] Yaw setpoint after applying slew rate
-float32 pid_yaw_rate_integral  	        # Integral of the PID for the closed loop yaw rate controller
-float32 pid_yaw_integral       	        # Integral of the PID for the closed loop yaw controller
-float32 pid_forward_throttle_integral   # Integral of the PID for the closed loop forward speed controller
-float32 pid_lateral_throttle_integral   # Integral of the PID for the closed loop lateral speed controller
+float32 measured_forward_speed        # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 measured_lateral_speed        # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
+float32 adjusted_yaw_rate_setpoint    # [rad/s] Yaw rate setpoint output of the closed loop yaw controller
+float32 measured_yaw_rate  	      # [rad/s] Measured yaw rate
+float32 measured_yaw        	      # [rad] Measured yaw
+float32 pid_yaw_rate_integral  	      # Integral of the PID for the closed loop yaw rate controller
+float32 pid_yaw_integral       	      # Integral of the PID for the closed loop yaw controller
+float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller
+float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller

 # TOPICS rover_mecanum_status
@@ -328,7 +328,7 @@ if (TARGET parameters_xml AND TARGET airframes_xml)
 	add_custom_command(
 		OUTPUT ${fw_package}
 		COMMAND
-			${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_mkfw.py
+			${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_mkfw.py
 				--prototype ${PX4_SOURCE_DIR}/boards/${PX4_BOARD_VENDOR}/${PX4_BOARD_MODEL}/firmware.prototype
 				--git_identity ${PX4_SOURCE_DIR}
 				--parameter_xml ${PX4_BINARY_DIR}/parameters.xml
@@ -6,13 +6,13 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
 export APPSDIR="`pwd`/../apps"

 if [ "command -v python3" ]; then
-	PYTHON_EXECUTABLE=python3
+	Python_EXECUTABLE=python3
 else
-	PYTHON_EXECUTABLE=python
+	Python_EXECUTABLE=python
 fi

 if [ "${1}" = "--olddefconfig" ]; then
-	PYTHONPATH=${DIR} ${PYTHON_EXECUTABLE} ${DIR}/olddefconfig.py > /dev/null
+	PYTHONPATH=${DIR} ${Python_EXECUTABLE} ${DIR}/olddefconfig.py > /dev/null
 else
 	echo "ERROR: ${@} unsupported"
 	exit 1
@@ -81,7 +81,7 @@ endif()
 string(REPLACE ";" "," serial_ports "${serial_ports}")

 add_custom_target(upload
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${fw_package}
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${fw_package}
 	DEPENDS ${fw_package}
 	COMMENT "uploading px4"
 	VERBATIM
@@ -90,7 +90,7 @@ add_custom_target(upload
 	)

 add_custom_target(force-upload
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --force --port ${serial_ports} ${fw_package}
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --force --port ${serial_ports} ${fw_package}
 	DEPENDS ${fw_package}
 	COMMENT "uploading px4 with --force"
 	VERBATIM
@@ -11,7 +11,6 @@ menu "barometer"
        select DRIVERS_BAROMETER_MS5611
        select DRIVERS_BAROMETER_MAIERTEK_MPC2520
        select DRIVERS_BAROMETER_GOERTEK_SPL06
-        select DRIVERS_BAROMETER_GOERTEK_SPA06
        select DRIVERS_BAROMETER_INVENSENSE_ICP101XX
        select DRIVERS_BAROMETER_INVENSENSE_ICP201XX
        ---help---
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022-2024 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022 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
@@ -32,4 +32,3 @@
 ############################################################################

 add_subdirectory(spl06)
-add_subdirectory(spa06)
@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE drivers__barometer__spa06
-	MAIN spa06
-	SRCS
-		SPA06.cpp
-		SPA06.hpp
-		SPA06_I2C.cpp
-		SPA06_SPI.cpp
-		spa06_main.cpp
-	DEPENDS
-		px4_work_queue
-	)
@@ -1,5 +0,0 @@
-menuconfig DRIVERS_BAROMETER_GOERTEK_SPA06
-	bool "spa06"
-	default n
-	---help---
-		Enable support for spa06
@@ -1,259 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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 "SPA06.hpp"
-
-SPA06::SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface) :
-	I2CSPIDriver(config),
-	_interface(interface),
-	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
-	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
-	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
-{
-}
-
-SPA06::~SPA06()
-{
-	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_measure_perf);
-	perf_free(_comms_errors);
-
-	delete _interface;
-}
-
-/*
-float
-SPA06::scale_factor(int oversampling_rate)
-{
-	float k;
-
-	switch (oversampling_rate) {
-	case 1:
-		k = 524288.0f;
-		break;
-
-	case 2:
-		k = 1572864.0f;
-		break;
-
-	case 4:
-		k = 3670016.0f;
-		break;
-
-	case 8:
-		k = 7864320.0f;
-		break;
-
-	case 16:
-		k = 253952.0f;
-		break;
-
-	case 32:
-		k = 516096.0f;
-		break;
-
-	case 64:
-		k = 1040384.0f;
-		break;
-
-	case 128:
-		k = 2088960.0f;
-		break;
-
-	default:
-		k = 0;
-		break;
-	}
-
-	return k;
-}
-*/
-
-int
-SPA06::calibrate()
-{
-	uint8_t buf[21];
-
-	_interface->read(SPA06_ADDR_CAL, buf, sizeof(buf));
-
-	_cal.c0 = (uint16_t)(buf[0]) << 4 | (uint16_t)(buf[1]) >> 4;
-	// If value is negative, we need to fill the missing bits.
-	_cal.c0 = (_cal.c0 & 1 << 11) ? (0xf000 | _cal.c0) : _cal.c0;
-
-	_cal.c1 = (uint16_t)(buf[1] & 0x0F) << 8 | buf[2];
-	_cal.c1 = (_cal.c1 & 1 << 11) ? (0xf000 | _cal.c1) : _cal.c1;
-
-	_cal.c00 = (uint32_t)(buf[3]) << 12 | (uint32_t)(buf[4]) << 4 | (buf[5]) >> 4;
-	_cal.c00 = (_cal.c00 & 1 << 19) ? (0xfff00000 | _cal.c00) : _cal.c00;
-
-	_cal.c10 = (uint32_t)(buf[5] & 0x0F) << 16 | (uint32_t)(buf[6]) << 8 | buf[7];
-	_cal.c10 = (_cal.c10 & 1 << 19) ? (0xfff00000 | _cal.c10) : _cal.c10;
-
-	_cal.c01 = (uint16_t)(buf[8]) << 8 | buf[9];
-
-	_cal.c11 = (uint16_t)(buf[10]) << 8 | buf[11];
-
-	_cal.c20 = (uint16_t)(buf[12]) << 8 | buf[13];
-
-	_cal.c21 = (uint16_t)(buf[14]) << 8 | buf[15];
-
-	_cal.c30 = (uint16_t)(buf[16]) << 8 | buf[17];
-
-	_cal.c31 = (uint16_t)(buf[18]) << 4 | (uint16_t)(buf[19] & 0xF0) >> 4;
-	_cal.c31 = (_cal.c31 & 1 << 11) ? (0xf000 | _cal.c31) : _cal.c31;
-
-	_cal.c40 = (uint16_t)(buf[19] & 0x0F) << 8 | buf[20];
-	_cal.c40 = (_cal.c40 & 1 << 11) ? (0xf000 | _cal.c40) : _cal.c40;
-
-	PX4_DEBUG("c0:%d\nc1:%d\nc00:%ld\nc10:%ld\nc01:%d\nc11:%d\nc20:%d\nc21:%d\nc30:%d\nc31:%d\nc40:%d\n",
-		  _cal.c0, _cal.c1,
-		  _cal.c00, _cal.c10,
-		  _cal.c01, _cal.c11, _cal.c20, _cal.c21, _cal.c30, _cal.c31, _cal.c40);
-
-	return OK;
-}
-int
-SPA06::init()
-{
-	int8_t tries = 5;
-	// reset sensor
-	_interface->set_reg(SPA06_VALUE_RESET, SPA06_ADDR_RESET);
-	usleep(10000);
-
-	// check id
-	if (_interface->get_reg(SPA06_ADDR_ID) != SPA06_VALUE_ID) {
-		PX4_DEBUG("id of your baro is not: 0x%02x", SPA06_VALUE_ID);
-		return -EIO;
-	}
-
-	while (tries--) {
-		uint8_t meas_cfg = _interface->get_reg(SPA06_ADDR_MEAS_CFG);
-
-		if (meas_cfg & (1 << 7) && meas_cfg & (1 << 6)) {
-			break;
-		}
-
-		usleep(10000);
-	}
-
-	if (tries < 0) {
-		PX4_DEBUG("spa06 sensor or coef not ready");
-		return -EIO;
-	}
-
-	// get calibration and pre process them
-	calibrate();
-
-	// set config, recommended settings
-	_interface->set_reg(_curr_prs_cfg, SPA06_ADDR_PRS_CFG);
-	kp = 253952.0f; // refer to scale_factor()
-	_interface->set_reg(_curr_tmp_cfg, SPA06_ADDR_TMP_CFG);
-	kt = 524288.0f;
-
-	// Enable FIFO
-	_interface->set_reg(1 << 2, SPA06_ADDR_CFG_REG);
-	// Continuous pressure and temperature mesasurement.
-	_interface->set_reg(7, SPA06_ADDR_MEAS_CFG);
-
-	Start();
-
-	return OK;
-}
-
-void
-SPA06::Start()
-{
-	// schedule a cycle to start things
-	ScheduleNow();
-}
-
-void
-SPA06::RunImpl()
-{
-	collect();
-
-	ScheduleDelayed(_measure_interval);
-}
-int
-SPA06::collect()
-{
-	perf_begin(_sample_perf);
-
-	// this should be fairly close to the end of the conversion, so the best approximation of the time
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-
-	if (_interface->read(SPA06_ADDR_DATA, (uint8_t *)&_data, sizeof(_data)) != OK) {
-		perf_count(_comms_errors);
-		perf_cancel(_sample_perf);
-		return -EIO;
-	}
-
-	int32_t temp_raw = (uint32_t)_data.t_msb << 16 | (uint32_t)_data.t_lsb << 8 | (uint32_t)_data.t_xlsb;
-	temp_raw = (temp_raw & 1 << 23) ? (0xff000000 | temp_raw) : temp_raw;
-
-	int32_t press_raw = (uint32_t)_data.p_msb << 16 | (uint32_t) _data.p_lsb << 8 | (uint32_t) _data.p_xlsb;
-	press_raw = (press_raw & 1 << 23) ? (0xff000000 | press_raw) : press_raw;
-
-	// calculate
-	float ftsc = (float)temp_raw / kt;
-	float fpsc = (float)press_raw / kp;
-	float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * ((float)_cal.c30) + fpsc * (float)_cal.c40);
-	float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * ((float)_cal.c21) + fpsc * (float)_cal.c31);
-
-	float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3;
-	float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc;
-
-
-	sensor_baro_s sensor_baro{};
-	sensor_baro.timestamp_sample = timestamp_sample;
-	sensor_baro.device_id = _interface->get_device_id();
-	sensor_baro.pressure = fp;
-	sensor_baro.temperature = temperature;
-	sensor_baro.error_count = perf_event_count(_comms_errors);
-	sensor_baro.timestamp = hrt_absolute_time();
-	_sensor_baro_pub.publish(sensor_baro);
-
-	perf_end(_sample_perf);
-
-	return OK;
-}
-
-void
-SPA06::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_measure_perf);
-	perf_print_counter(_comms_errors);
-}
@@ -1,117 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include "spa06.h"
-
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <lib/perf/perf_counter.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/sensor_baro.h>
-
-class SPA06 : public I2CSPIDriver<SPA06>
-{
-public:
-	SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface);
-	virtual ~SPA06();
-
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
-
-	int			init();
-	void			print_status();
-
-	void			RunImpl();
-private:
-	void			Start();
-	// float scale_factor(int oversampling_rate);
-
-	int			collect(); //get results and publish
-	int			calibrate();
-
-	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
-
-	spa06::ISPA06		*_interface;
-	spa06::data_s		_data;
-	spa06::calibration_s	_cal{};
-
-	// set config, recommended settings
-	//
-	// oversampling rate  : single | 2       | 4       | 8       | 16     | 32     | 64      | 128
-	// scale factor(KP/KT): 524288 | 1572864 | 3670016 | 7864320 | 253952 | 516096 | 1040384 | 2088960
-
-	// configuration of pressure measurement rate (PM_RATE) and resolution (PM_PRC)
-	//
-	// PM_RATE[7:4]    : 0 | 1 | 2 | 3 | 4  | 5  | 6  | 7   | 8     | 9    | 10   | 11   | 12 | 13 | 14  | 15
-	// measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200
-	// note: applicable for measurements in background mode only
-	//
-	// PM_PRC[3:0]         : 0      | 1   | 2   | 3    | 4    | 5    | 6     | 7
-	// oversampling (times): single | 2   | 4   | 8    | 16   | 32   | 64    | 128
-	// measurement time(ms): 3.6    | 5.2 | 8.4 | 14.8 | 27.6 | 53.2 | 104.4 | 206.8
-	// precision(PaRMS)    : 5.0    |     | 2.5 |      | 1.2  | 0.9  | 0.5   |
-	// note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register
-	//
-	// -> 32 measurements per second, 16 oversampling
-	static constexpr uint8_t	_curr_prs_cfg{5 << 4 | 4};
-
-	// configuration of temperature measurment rate (TMP_RATE) and resolution (TMP_PRC)
-	//
-	// temperature measurement: internal sensor (in ASIC) | external sensor (in pressure sensor MEMS element)
-	// PM_RATE[7:4]    : 0 | 1 | 2 | 3 | 4  | 5  | 6  | 7   | 8     | 9    | 10   | 11   | 12 | 13 | 14  | 15
-	// measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200
-	// note: applicable for measurements in background mode only
-	//
-	// TMP_PRC[2:0]        : 0      | 1 | 2 | 3 | 4  | 5  | 6  | 7
-	// oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128
-	// note: single(default) measurement time 3.6ms, other settings are optional, and may not be relevant
-	// note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register
-
-	// -> 32 measurements per second, single oversampling
-	static constexpr uint8_t	_curr_tmp_cfg{5 << 4 | 0};
-
-	bool			_collect_phase{false};
-	float kp;
-	float kt;
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_measure_perf;
-	perf_counter_t		_comms_errors;
-
-	static constexpr uint32_t _sample_rate{32};
-	static constexpr uint32_t _measure_interval{1000000 / _sample_rate};
-};
@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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 SPA06_I2C.cpp
- *
- * SPI interface for Goertek SPA06
- */
-
-#include "spa06.h"
-
-#include <px4_platform_common/px4_config.h>
-#include <drivers/device/i2c.h>
-
-#if defined(CONFIG_I2C)
-
-class SPA06_I2C: public device::I2C, public spa06::ISPA06
-{
-public:
-	SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency);
-	virtual ~SPA06_I2C() override = default;
-
-	int init() override { return I2C::init(); }
-
-	uint8_t	get_reg(uint8_t addr) override;
-	int	set_reg(uint8_t value, uint8_t addr) override;
-
-	int read(uint8_t addr, uint8_t *buf, uint8_t len) override;
-	//spa06::data_s		*get_data(uint8_t addr) override;
-	//spa06::calibration_s	*get_calibration(uint8_t addr) override;
-
-	uint32_t get_device_id() const override { return device::I2C::get_device_id(); }
-
-	uint8_t get_device_address() const override { return device::I2C::get_device_address(); }
-private:
-	spa06::calibration_s	_cal{};
-	spa06::data_s		_data{};
-};
-
-spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency)
-{
-	return new SPA06_I2C(busnum, device, bus_frequency);
-}
-
-SPA06_I2C::SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency) :
-	I2C(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, bus_frequency)
-{
-}
-
-uint8_t
-SPA06_I2C::get_reg(uint8_t addr)
-{
-	uint8_t cmd[2] = { (uint8_t)(addr), 0};
-	transfer(&cmd[0], 1, &cmd[1], 1);
-
-	return cmd[1];
-}
-
-int
-SPA06_I2C::set_reg(uint8_t value, uint8_t addr)
-{
-	uint8_t cmd[2] = { (uint8_t)(addr), value};
-	return transfer(cmd, sizeof(cmd), nullptr, 0);
-}
-
-int
-SPA06_I2C::read(uint8_t addr, uint8_t *buf, uint8_t len)
-{
-	return transfer(&addr, 1, buf, len);
-}
-
-#endif // CONFIG_I2C
@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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 SPA06_SPI.cpp
- *
- * SPI interface for Goertek SPA06
- */
-
-#include "spa06.h"
-
-#include <px4_platform_common/px4_config.h>
-#include <drivers/device/spi.h>
-
-#if defined(CONFIG_SPI)
-
-/* SPI protocol address bits */
-#define DIR_READ			(1<<7)  //for set
-#define DIR_WRITE			~(1<<7) //for clear
-
-class SPA06_SPI: public device::SPI, public spa06::ISPA06
-{
-public:
-	SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
-	virtual ~SPA06_SPI() override = default;
-
-	int init() override { return SPI::init(); }
-
-	uint8_t	get_reg(uint8_t addr) override;
-	int	set_reg(uint8_t value, uint8_t addr) override;
-
-	int read(uint8_t addr, uint8_t *buf, uint8_t len) override;
-
-	uint32_t get_device_id() const override { return device::SPI::get_device_id(); }
-
-	uint8_t get_device_address() const override { return device::SPI::get_device_address(); }
-};
-
-spa06::ISPA06 *
-spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode)
-{
-	return new SPA06_SPI(busnum, device, bus_frequency, spi_mode);
-}
-
-SPA06_SPI::SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, spi_mode, bus_frequency)
-{
-}
-
-uint8_t
-SPA06_SPI::get_reg(uint8_t addr)
-{
-	uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; // set MSB bit
-	transfer(&cmd[0], &cmd[0], 2);
-
-	return cmd[1];
-}
-
-int
-SPA06_SPI::set_reg(uint8_t value, uint8_t addr)
-{
-	uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; // clear MSB bit
-	return transfer(&cmd[0], nullptr, 2);
-}
-
-int
-SPA06_SPI::read(uint8_t addr, uint8_t *buf, uint8_t len)
-{
-	uint8_t tx_buf[len + 1] = {(uint8_t)(addr | DIR_READ)}; // GCC support VLA, let's use it
-
-	return transfer(tx_buf, buf, len);
-}
-
-#endif // CONFIG_SPI
@@ -1,41 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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.
- *
- ****************************************************************************/
-
-/**
- * Goertek SPA06 Barometer (external I2C)
- *
- * @reboot_required true
- * @group Sensors
- * @boolean
-  */
-PARAM_DEFINE_INT32(SENS_EN_SPA06, 0);
@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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 spa06.h
- *
- * Shared defines for the SPA06 driver.
- */
-#pragma once
-
-#include <px4_platform_common/i2c_spi_buses.h>
-
-#define SPA06_ADDR_ID	0x0d
-#define SPA06_ADDR_RESET 0x0c   // set to reset
-#define SPA06_ADDR_CAL 0x10
-#define SPA06_ADDR_PRS_CFG  0x06
-#define SPA06_ADDR_TMP_CFG  0x07
-#define SPA06_ADDR_MEAS_CFG	0x08
-#define SPA06_ADDR_CFG_REG	0x09
-#define SPA06_ADDR_DATA 0x00
-
-
-#define SPA06_VALUE_RESET 9
-#define SPA06_VALUE_ID    0x11
-
-namespace spa06
-{
-
-#pragma pack(push,1)
-struct calibration_s {
-	int16_t c0, c1;
-	int32_t c00, c10;
-	int16_t c01, c11, c20, c21, c30, c31, c40;
-};
-
-struct data_s {
-	uint8_t p_msb;
-	uint8_t p_lsb;
-	uint8_t p_xlsb;
-
-	uint8_t t_msb;
-	uint8_t t_lsb;
-	uint8_t t_xlsb;
-};
-#pragma pack(pop)
-
-class ISPA06
-{
-public:
-	virtual ~ISPA06() = default;
-
-	virtual int init() = 0;
-
-	// read reg value
-	virtual uint8_t get_reg(uint8_t addr) = 0;
-
-	// write reg value
-	virtual int set_reg(uint8_t value, uint8_t addr) = 0;
-
-	// bulk read of data into buffer, return same pointer
-	virtual int read(uint8_t addr, uint8_t *buf, uint8_t len) = 0;
-	// bulk read of calibration data into buffer, return same pointer
-
-	virtual uint32_t get_device_id() const = 0;
-
-	virtual uint8_t get_device_address() const = 0;
-};
-
-} // namespace spa06
-
-
-#if defined(CONFIG_SPI)
-extern spa06::ISPA06 *spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
-#endif // CONFIG_SPI
-#if defined(CONFIG_I2C)
-extern spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency);
-#endif // CONFIG_I2C
@@ -1,141 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 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 <px4_platform_common/px4_config.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
-
-#include "SPA06.hpp"
-
-#include <drivers/drv_sensor.h>
-
-extern "C" { __EXPORT int spa06_main(int argc, char *argv[]); }
-
-void
-SPA06::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("spa06", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
-	PRINT_MODULE_USAGE_COMMAND("start");
-#if defined(CONFIG_I2C)
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
-#else
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-#endif
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-I2CSPIDriverBase *SPA06::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	spa06::ISPA06 *interface = nullptr;
-
-#if defined(CONFIG_I2C)
-
-	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = spa06_i2c_interface(config.bus, config.i2c_address, config.bus_frequency);
-
-	}
-
-#endif // CONFIG_I2C
-#if defined(CONFIG_SPI)
-
-	if (config.bus_type == BOARD_SPI_BUS) {
-		interface = spa06_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
-	}
-
-#endif // CONFIG_SPI
-
-	if (interface == nullptr) {
-		PX4_ERR("failed creating interface for bus %i", config.bus);
-		return nullptr;
-	}
-
-	if (interface->init() != OK) {
-		delete interface;
-		PX4_DEBUG("no device on bus %i", config.bus);
-		return nullptr;
-	}
-
-	SPA06 *dev = new SPA06(config, interface);
-
-	if (dev == nullptr) {
-		delete interface;
-		return nullptr;
-	}
-
-	if (OK != dev->init()) {
-		delete dev;
-		return nullptr;
-	}
-
-	return dev;
-}
-
-int
-spa06_main(int argc, char *argv[])
-{
-	using ThisDriver = SPA06;
-	BusCLIArguments cli{true, true};
-#if defined(CONFIG_I2C)
-	cli.i2c_address = 0x76;
-	cli.default_i2c_frequency = 100 * 1000;
-#endif // CONFIG_I2C
-#if defined(CONFIG_SPI)
-	cli.default_spi_frequency = 10 * 1000 * 1000;
-#endif // CONFIG_SPI
-
-	const char *verb = cli.parseDefaultArguments(argc, argv);
-
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_SPA06);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}
@@ -71,7 +71,7 @@ if(NOT "${PX4_BOARD_LABEL}" MATCHES  "canbootloader")
 			${PX4_BINARY_DIR}/${uavcan_bl_image_name}
 			${PX4_BINARY_DIR}/deploy/${HWBOARD_ID}.bin
 		COMMAND
-			${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py -v --padding ${uavcanbl_padding} --use-git-hash ${PX4_CONFIG}.bin ${uavcan_bl_image_name}
+			${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py -v --padding ${uavcanbl_padding} --use-git-hash ${PX4_CONFIG}.bin ${uavcan_bl_image_name}
 		COMMAND
 			 COMMAND ${CMAKE_COMMAND} -E make_directory deploy
 		COMMAND
@@ -32,24 +32,24 @@
 ****************************************************************************/

 #include "lightware_sf45_serial.hpp"
+#include "sf45_commands.h"

 #include <inttypes.h>
 #include <fcntl.h>
 #include <termios.h>
 #include <lib/crc/crc.h>
-#include <lib/mathlib/mathlib.h>

 #include <float.h>
-
-using namespace time_literals;
+#include <mathlib/mathlib.h>
+#include <matrix/matrix/math.hpp>

 /* Configuration Constants */
+#define SF45_MAX_PAYLOAD 256
 #define SF45_SCALE_FACTOR 0.01f
-using namespace matrix;

-SF45LaserSerial::SF45LaserSerial(const char *port) :
+SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_px4_rangefinder(0, distance_sensor_s::ROTATION_CUSTOM),
+	_px4_rangefinder(0, rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
 {
@@ -68,9 +68,16 @@ SF45LaserSerial::SF45LaserSerial(const char *port) :
 		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);
-	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_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;

 }

@@ -90,41 +97,47 @@ int SF45LaserSerial::init()
 	param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);

 	/* SF45/B (50M) */
-	_px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_CUSTOM);
 	_px4_rangefinder.set_min_distance(0.2f);
 	_px4_rangefinder.set_max_distance(50.0f);
 	_interval = 10000;

 	start();
+
 	return PX4_OK;
 }

 int SF45LaserSerial::measure()
 {
-	int32_t rate = (int32_t)_update_rate;
-	_data_output = 0x101; // raw distance (first return) + yaw readings
+
+	int rate = (int)_update_rate;
+	_data_output = 0x101; // raw distance + yaw readings
 	_stream_data = 5; // enable constant streaming

-	// send packets to the sensor depending on the state
+	// send some packets so the sensor starts scanning
 	switch (_sensor_state) {

+	// sensor should now respond
 	case STATE_UNINIT:
-		// Used to probe if the sensor is alive
-		sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
+		while (_num_retries--) {
+			sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
+			_sensor_state = STATE_UNINIT;
+		}
+
+		_sensor_state = STATE_SEND_PRODUCT_NAME;
 		break;

-	case STATE_ACK_PRODUCT_NAME:
+	case STATE_SEND_PRODUCT_NAME:
 		// Update rate default to 50 readings/s
 		sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t));
+		_sensor_state = STATE_SEND_UPDATE_RATE;
 		break;

-	case STATE_ACK_UPDATE_RATE:
-		// Configure the data that the sensor shall output
+	case STATE_SEND_UPDATE_RATE:
 		sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output));
+		_sensor_state = STATE_SEND_DISTANCE_DATA;
 		break;

-	case STATE_ACK_DISTANCE_OUTPUT:
-		// Configure the sensor to automatically output data at the configured update rate
+	case STATE_SEND_DISTANCE_DATA:
 		sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data));
 		_sensor_state = STATE_SEND_STREAM;
 		break;
@@ -138,90 +151,129 @@ int SF45LaserSerial::measure()

 int SF45LaserSerial::collect()
 {
+	perf_begin(_sample_perf);
+
+	/* clear buffer if last read was too long ago */
+	int ret;
+	/* the buffer for read chars is buflen minus null termination */
+	uint8_t readbuf[SF45_MAX_PAYLOAD];
+
 	float distance_m = -1.0f;

-	if (_sensor_state == STATE_UNINIT) {
+	/* read from the sensor (uart buffer) */
+	const hrt_abstime timestamp_sample = hrt_absolute_time();

-		perf_begin(_sample_perf);
-		const int payload_length = 22;

-		_crc_valid = false;
-		sf45_get_and_handle_request(payload_length, SF_PRODUCT_NAME);

-		if (_crc_valid) {
-			_sensor_state = STATE_ACK_PRODUCT_NAME;
+	if (_sensor_state == STATE_SEND_PRODUCT_NAME) {
+
+		ret = ::read(_fd, &readbuf[0], 22);
+
+		if (ret < 0) {
+			PX4_ERR("ERROR (ack from sending product name cmd): %d", ret);
+			perf_count(_comms_errors);
 			perf_end(_sample_perf);
-			return PX4_OK;
+			return ret;
 		}

-		return -EAGAIN;
+		sf45_request_handle(ret, readbuf);
+		ScheduleDelayed(_interval * 3);

-	} else if (_sensor_state == STATE_ACK_PRODUCT_NAME) {
+	} else if (_sensor_state == STATE_SEND_UPDATE_RATE) {

-		perf_begin(_sample_perf);
-		const int payload_length = 7;
+		ret = ::read(_fd, &readbuf[0], 7);

-		_crc_valid = false;
-		sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE);
-
-		if (_crc_valid) {
-			_sensor_state = STATE_ACK_UPDATE_RATE;
+		if (ret < 0) {
+			PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret);
+			perf_count(_comms_errors);
 			perf_end(_sample_perf);
-			return PX4_OK;
+			return ret;
 		}

-		return -EAGAIN;
-
-	} else if (_sensor_state == STATE_ACK_UPDATE_RATE) {
-
-		perf_begin(_sample_perf);
-		const int payload_length = 10;
-
-		_crc_valid = false;
-		sf45_get_and_handle_request(payload_length, SF_DISTANCE_OUTPUT);
-
-		if (_crc_valid) {
-			_sensor_state = STATE_ACK_DISTANCE_OUTPUT;
-			perf_end(_sample_perf);
-			return PX4_OK;
+		if (readbuf[3] == SF_UPDATE_RATE) {
+			sf45_request_handle(ret, readbuf);
+			ScheduleDelayed(_interval * 3);
 		}

-		return -EAGAIN;
+	} else if (_sensor_state == STATE_SEND_DISTANCE_DATA) {
+
+		ret = ::read(_fd, &readbuf[0], 8);
+
+		if (ret < 0) {
+			PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret);
+			perf_count(_comms_errors);
+			perf_end(_sample_perf);
+			return ret;
+		}
+
+		if (readbuf[3] == SF_DISTANCE_OUTPUT) {
+			sf45_request_handle(ret, readbuf);
+			ScheduleDelayed(_interval * 3);
+		}
+
+		// Stream data from sensor

 	} else {

-		// Stream data from sensor
-		perf_begin(_sample_perf);
-		const int payload_length = 10;
+		ret = ::read(_fd, &readbuf[0], 10);

-		_crc_valid = false;
-		sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
-
-		if (_crc_valid) {
-			sf45_process_replies(distance_m);
-			PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
+		if (ret < 0) {
+			PX4_ERR("ERROR (ack from streaming distance data): %d", ret);
+			perf_count(_comms_errors);
 			perf_end(_sample_perf);
-			return PX4_OK;
+			return ret;
 		}

+		uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2);
+
+		// Process the incoming distance data
+		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_ERR("ERROR (unknown sensor data): %d", ret);
+				perf_count(_comms_errors);
+				perf_end(_sample_perf);
+				return ret;
+			}
+		}
+	}
+
+	if (_consecutive_fail_count > 35 && !_sensor_ready) {
+		PX4_ERR("Restarting the state machine");
+		return PX4_ERROR;
+	}
+
+	_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 sensor state */
-	_sensor_state = STATE_UNINIT;
-
-	/* reset the report ring */
+	/* reset the report ring and state machine */
 	_collect_phase = false;

-	/* reset the UART receive buffer size */
-	_linebuf_size = 0;
-
-	/* reset the fail counter */
-	_last_received_time = hrt_absolute_time();
-
 	/* schedule a cycle to start things */
 	ScheduleNow();
 }
@@ -236,7 +288,8 @@ void SF45LaserSerial::Run()
 	/* fds initialized? */
 	if (_fd < 0) {
 		/* open fd: non-blocking read mode*/
-		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+		_fd = ::open(_port, O_RDWR | O_NOCTTY);

 		if (_fd < 0) {
 			PX4_ERR("serial open failed (%i)", errno);
@@ -250,11 +303,34 @@ void SF45LaserSerial::Run()
 		/* fill the struct for the new configuration */
 		tcgetattr(_fd, &uart_config);

-		/* clear ONLCR flag (which appends a CR for every LF) */
-		uart_config.c_oflag &= ~ONLCR;
+		uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8;

-		/* no parity, one stop bit */
-		uart_config.c_cflag &= ~(CSTOPB | PARENB);
+		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;

@@ -270,37 +346,51 @@ void SF45LaserSerial::Run()
 		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
 			PX4_ERR("baud %d ATTR", termios_state);
 		}
+
 	}

 	if (_collect_phase) {
-		if (hrt_absolute_time() - _last_received_time >= 1_s) {
-			start();
+
+		/* 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;
 		}

-		/* perform collection */
-		if (collect() != PX4_OK && errno != EAGAIN) {
-			PX4_DEBUG("collect error");
+		if (OK != collect_ret) {
+			//  Too many packet errors in init, restart the consecutive fail count
+			_consecutive_fail_count = 0;
+
+			/* restart the measurement state machine */
+			start();
+			return;
+
+		} else {
+			/* apparently success */
+			_consecutive_fail_count = 0;
 		}

-		if (_sensor_state != STATE_SEND_STREAM) {
-			/* next phase is measurement */
-			_collect_phase = false;
-		}
-
-	} else {
-		/* measurement phase */
-
-		if (measure() != PX4_OK) {
-			PX4_DEBUG("measure error");
-		}
-
-		/* next phase is collection */
-		_collect_phase = true;
+		/* 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()
@@ -309,8 +399,9 @@ void SF45LaserSerial::print_info()
 	perf_print_counter(_comms_errors);
 }

-void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id)
+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
@@ -319,178 +410,172 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
 	// ID byte precedes the data in the payload
 	// CRC comprised of 16-bit checksum (not included in checksum calc.)

-	int ret;
-	size_t max_read = sizeof(_linebuf) - _linebuf_size;
-	ret = ::read(_fd, &_linebuf[_linebuf_size], max_read);
+	uint16_t recv_crc = 0;
+	bool restart_flag = false;

-	if (ret < 0 && errno != EAGAIN) {
-		PX4_ERR("ERROR (ack from streaming distance data): %d", ret);
-		_linebuf_size = 0;
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return;
-	}
+	while (restart_flag != true) {

-	if (ret > 0) {
-		_last_received_time = hrt_absolute_time();
-		_linebuf_size += ret;
-	}
+		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;

-	// Not enough data to parse a complete packet. Gather more data in the next cycle.
-	if (_linebuf_size < payload_length) {
-		return;
-	}
+				} else {
+					_sop_valid = false;
+					_crc_valid = false;
+					_parsed_state = 0;
+					restart_flag = true;
+					_calc_crc = 0;
+					perf_count(_comms_errors);
+					perf_end(_sample_perf);
+					PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
+					_consecutive_fail_count++;
+					break;
+				} // end else
+			} // end case 0

-	int index = 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;
+			}

-	while (index <= _linebuf_size - payload_length && _crc_valid == false) {
-		bool restart_flag = false;
+		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);

-		while (restart_flag != true) {
-			switch (_parsed_state) {
-			case 0: {
-					if (_linebuf[index] == 0xAA) {
-						// start of frame is valid, continue
-						_sop_valid = true;
-						_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
-						_parsed_state = 1;
-						break;
+				// Check payload length against known max value
+				if (rx_field.data_len > 17) {
+					_parsed_state = 0;
+					restart_flag = true;
+					_calc_crc = 0;
+					perf_count(_comms_errors);
+					perf_end(_sample_perf);
+					PX4_DEBUG("Payload length error: %d", _sensor_state);
+					_consecutive_fail_count++;
+					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 {
-						_sop_valid = false;
-						_crc_valid = false;
-						_parsed_state = 0;
-						restart_flag = true;
-						_calc_crc = 0;
-						PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
-						break;
-					} // end else
-				} // end case 0
+						_sensor_ready = false;
+					}

-			case 1: {
-					rx_field.flags_lo = _linebuf[index + 1];
-					_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
-					_parsed_state = 2;
+					_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
+
+					_parsed_state = 4;
 					break;
 				}

-			case 2: {
-					rx_field.flags_hi = _linebuf[index + 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);
+				// Ignore message ID's that aren't defined
+				else {
+					_parsed_state = 0;
+					_calc_crc = 0;
+					restart_flag = true;
+					perf_count(_comms_errors);
+					perf_end(_sample_perf);
+					_consecutive_fail_count++;
+					PX4_DEBUG("Unknown message ID: %d", _sensor_state);
+					break;

-					// Check payload length against known max value
-					if (rx_field.data_len > 17) {
-						_parsed_state = 0;
-						restart_flag = true;
-						_calc_crc = 0;
-						PX4_DEBUG("Payload length error: %d", _sensor_state);
-						break;
-
-					} else {
-						_parsed_state = 3;
-						break;
-					}
 				}
+			}

-			case 3: {
-					rx_field.msg_id = _linebuf[index + 3];
+		// 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) {

-					if (rx_field.msg_id == msg_id) {
-						_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
-						_parsed_state = 4;
-						break;
-					}
+						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;

-					// Ignore message ID's that aren't searched
-					else {
-						_parsed_state = 0;
-						_calc_crc = 0;
-						restart_flag = true;
-						PX4_DEBUG("Non needed message ID: %d", _sensor_state);
-						break;
-					}
-				}
+					}  // end for
+				} //end if

-			// 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] = _linebuf[index + 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);
-					}
+				else {

 					_parsed_state = 5;
 					_data_bytes_recv = 0;
-					break;
+					_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
+
 				}

-			// CRC low byte
-			case 5: {
-					rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len];
-					_parsed_state = 6;
-					break;
-				}
+				_parsed_state = 5;
+				_data_bytes_recv = 0;
+				break;
+			}

-			// CRC high byte
-			case 6: {
-					rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len];
-					uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
+		// CRC low byte
+		case 5: {
+				rx_field.crc[0] = input_buf[3 + rx_field.data_len];
+				_parsed_state = 6;
+				break;
+			}

-					// 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;
-						_parsed_state = 0;
-						_calc_crc = 0;
-						restart_flag = true;
-						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 {
-
-						_crc_valid = false;
-						_parsed_state = 0;
-						_calc_crc = 0;
-						restart_flag = true;
-						perf_count(_comms_errors);
-						PX4_DEBUG("CRC mismatch: %d", _sensor_state);
-						break;
+						_init_complete = false;
 					}
+
+					_parsed_state = 0;
+					_calc_crc = 0;
+					restart_flag = true;
+					break;
+
+				} else {
+
+					_crc_valid = false;
+					_init_complete = false;
+					_parsed_state = 0;
+					_calc_crc = 0;
+					restart_flag = true;
+					perf_count(_comms_errors);
+					perf_end(_sample_perf);
+					PX4_DEBUG("CRC mismatch: %d", _sensor_state);
+					break;
 				}
-			} // end switch
-		} //end while
-
-		index++;
-	}
-
-	// If we parsed successfully, remove the parsed part from the buffer if it is still large enough
-	if (_crc_valid && index + payload_length < _linebuf_size) {
-		unsigned next_after_index = index + payload_length;
-		memmove(&_linebuf[0], &_linebuf[next_after_index], _linebuf_size - next_after_index);
-		_linebuf_size -= next_after_index;
-	}
-
-	// The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again.
-	if ((unsigned)_linebuf_size >= sizeof(_linebuf)) {
-		_linebuf_size = 0;
-		perf_count(_comms_errors);
-	}
+			}
+		} // end switch
+	} //end while
 }

-void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8_t data_len)
+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];
@@ -527,7 +612,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8
 	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;
+		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;
@@ -565,11 +650,12 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8
 		// 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_DEBUG("DEBUG: Product name");
+		PX4_INFO("INFO: Product name");
 	}

 	uint8_t crc_lo = crc_val & 0xFF;
@@ -597,11 +683,12 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8
 	}
 }

-void SF45LaserSerial::sf45_process_replies(float &distance_m)
+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;
@@ -609,10 +696,11 @@ void SF45LaserSerial::sf45_process_replies(float &distance_m)
 			// 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 == ROTATION_DOWNWARD_FACING) {
+			if (_orient_cfg == 1) {
 				raw_yaw = raw_yaw * -1;
 			}

@@ -620,10 +708,10 @@ void SF45LaserSerial::sf45_process_replies(float &distance_m)
 			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;

 			switch (_yaw_cfg) {
-			case ROTATION_FORWARD_FACING:
+			case 0:
 				break;

-			case ROTATION_BACKWARD_FACING:
+			case 1:
 				if (scaled_yaw > 180) {
 					scaled_yaw = scaled_yaw - 180;

@@ -633,11 +721,11 @@ void SF45LaserSerial::sf45_process_replies(float &distance_m)

 				break;

-			case ROTATION_RIGHT_FACING:
+			case 2:
 				scaled_yaw = scaled_yaw + 90; // rotation facing right
 				break;

-			case ROTATION_LEFT_FACING:
+			case 3:
 				scaled_yaw = scaled_yaw - 90; // rotation facing left
 				break;

@@ -645,21 +733,27 @@ void SF45LaserSerial::sf45_process_replies(float &distance_m)
 				break;
 			}

+			// Convert to meters for rangefinder update
+			*distance_m = raw_distance * SF45_SCALE_FACTOR;
+			obstacle_dist_cm = (uint16_t)raw_distance;

-			if (raw_distance < 65436) { // Discard invalid readings
-				// Convert to meters for rangefinder update
-				distance_m = raw_distance * SF45_SCALE_FACTOR;
+			uint8_t current_bin = sf45_convert_angle(scaled_yaw);

-				Quatf quaternion(Eulerf{0, 0, sf45_wrap_360(scaled_yaw)*M_DEG_TO_RAD_F});
-				float q[4];
+			// If we have moved to a new bin

-				for (int i = 0; i < 4; i++) {
-					q[i] = quaternion(i);
-				}
+			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();

-				_px4_rangefinder.update(hrt_absolute_time(), distance_m, -1, q);
 			}

+			_previous_bin = current_bin;
+
+			_obstacle_distance_pub.publish(_obstacle_map_msg);
+
 			break;
 		}

@@ -669,6 +763,16 @@ void SF45LaserSerial::sf45_process_replies(float &distance_m)
 	}
 }

+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);
@@ -56,37 +56,33 @@

 enum SF_SERIAL_STATE {
 	STATE_UNINIT = 0,
-	STATE_ACK_PRODUCT_NAME = 1,
-	STATE_ACK_UPDATE_RATE = 2,
-	STATE_ACK_DISTANCE_OUTPUT = 3,
+	STATE_SEND_PRODUCT_NAME = 1,
+	STATE_SEND_UPDATE_RATE = 2,
+	STATE_SEND_DISTANCE_DATA = 3,
 	STATE_SEND_STREAM = 4,
 };


-enum SensorOrientation {	  // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
-	ROTATION_FORWARD_FACING = 0,  // MAV_SENSOR_ROTATION_NONE
-	ROTATION_RIGHT_FACING = 2,    // MAV_SENSOR_ROTATION_YAW_90
-	ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
-	ROTATION_LEFT_FACING = 6,     // MAV_SENSOR_ROTATION_YAW_270
-	ROTATION_UPWARD_FACING = 24,  // MAV_SENSOR_ROTATION_PITCH_90
-	ROTATION_DOWNWARD_FACING = 25,// MAV_SENSOR_ROTATION_PITCH_270
-};
-using namespace time_literals;
 class SF45LaserSerial : public px4::ScheduledWorkItem
 {
 public:
-	SF45LaserSerial(const char *port);
+	SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	~SF45LaserSerial() override;

-	int				init();
+	int 			        init();
 	void				print_info();
-	void				sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id);
-	void				sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
-	uint16_t			sf45_format_crc(uint16_t crc, uint8_t data_value);
-	void				sf45_process_replies(float &data);
-	float				sf45_wrap_360(float f);
+	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;
@@ -95,34 +91,41 @@ private:
 	bool                            _crc_valid{false};
 	PX4Rangefinder                  _px4_rangefinder;

-
 	char 				_port[20] {};
-	int				_interval{2000};
+	int	        _interval{10000};
 	bool				_collect_phase{false};
 	int 				_fd{-1};
-	uint8_t				_linebuf[SF45_MAX_PAYLOAD] {};
-	int				_linebuf_size{0};
+	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};
-	SF_SERIAL_STATE			_sensor_state{STATE_UNINIT};
-	int				_baud_rate{0};
-	int32_t				_product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-	int32_t				_stream_data{0};
-	int32_t				_update_rate{0};
-	int32_t				_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};
-	int32_t				_yaw_cfg{0};
-	int32_t				_orient_cfg{0};
+	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{0};
+	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

-	hrt_abstime			_last_received_time{0};
-	perf_counter_t			_sample_perf;
-	perf_counter_t			_comms_errors;
+	unsigned			                  _consecutive_fail_count;
+
+	perf_counter_t			            _sample_perf;
+	perf_counter_t			            _comms_errors;
+
 };
@@ -41,7 +41,7 @@ namespace lightware_sf45

 SF45LaserSerial *g_dev{nullptr};

-static int start(const char *port)
+static int start(const char *port, uint8_t rotation)
 {
 	if (g_dev != nullptr) {
 		PX4_WARN("already started");
@@ -54,7 +54,7 @@ static int start(const char *port)
 	}

 	/* create the driver */
-	g_dev = new SF45LaserSerial(port);
+	g_dev = new SF45LaserSerial(port, rotation);

 	if (g_dev == nullptr) {
 		return -1;
@@ -102,7 +102,7 @@ static int usage()

 Serial bus driver for the Lightware SF45/b Laser rangefinder.

-Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html
+Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html

 ### Examples

@@ -116,6 +116,7 @@ $ lightware_sf45_serial stop
 	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;
 }
@@ -124,13 +125,18 @@ $ lightware_sf45_serial stop

 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, "d:", &myoptind, &myoptarg)) != EOF) {
+	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;
@@ -147,7 +153,7 @@ extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
 	}

 	if (!strcmp(argv[myoptind], "start")) {
-		return lightware_sf45::start(device_path);
+		return lightware_sf45::start(device_path, rotation);

 	} else if (!strcmp(argv[myoptind], "stop")) {
 		return lightware_sf45::stop();
@@ -32,7 +32,7 @@ parameters:
                  12: 5000hz
              reboot_required: true
              num_instances: 1
-              default: 5
+              default: 1

          SF45_ORIENT_CFG:
              description:
@@ -41,11 +41,11 @@ parameters:
                      The SF45 mounted facing upward or downward on the frame
              type: enum
              values:
-                24: Rotation upward
-                25: Rotation downward
+                0: Rotation upward
+                1: Rotation downward
              reboot_required: true
              num_instances: 1
-              default: 24
+              default: 0

          SF45_YAW_CFG:
              description:
@@ -55,9 +55,9 @@ parameters:
              type: enum
              values:
                0: Rotation forward
+                1: Rotation backward
                2: Rotation right
-                4: Rotation backward
-                6: Rotation left
+                3: Rotation left
              reboot_required: true
              num_instances: 1
              default: 0
@@ -247,7 +247,6 @@

 #define DRV_DIFF_PRESS_DEVTYPE_AUAV	0xE6
 #define DRV_BARO_DEVTYPE_AUAV		0xE7
-#define DRV_BARO_DEVTYPE_SPA06          0xE8

 #define DRV_DEVTYPE_UNUSED		0xff

@@ -120,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
 endforeach(DSDLC_INPUT)
 add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp
 	COMMAND
-		${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
+		${Python_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
 			--outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS}
 			#--verbose
 	COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
@@ -42,7 +42,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
    set(DSDLC_INPUT_FILES ${DSDLC_INPUT_FILES} ${DSDLC_NEW_INPUT_FILES})
 endforeach(DSDLC_INPUT)
 add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp
-                   COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
+                   COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
                   COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp
                   DEPENDS ${DSDLC_INPUT_FILES}
                   WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
@@ -120,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
 endforeach(DSDLC_INPUT)
 add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp
 	COMMAND
-		${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
+		${Python_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
 			--outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS}
 			#--verbose
 	COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
@@ -54,7 +54,6 @@ add_subdirectory(geo EXCLUDE_FROM_ALL)
 add_subdirectory(heatshrink EXCLUDE_FROM_ALL)
 add_subdirectory(hysteresis EXCLUDE_FROM_ALL)
 add_subdirectory(l1 EXCLUDE_FROM_ALL)
-add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)
@@ -83,12 +83,12 @@ list(APPEND comp_metadata_types "--type" "5,${PX4_BINARY_DIR}/actuators.json.xz,
 set(component_general_json ${PX4_BINARY_DIR}/component_general.json)
 set(component_information_header ${CMAKE_CURRENT_BINARY_DIR}/checksums.h)
 add_custom_command(OUTPUT ${component_general_json} ${component_general_json}.xz ${component_information_header}
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_component_general.py
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_component_general.py
 		${component_general_json}
 		--compress
 		${comp_metadata_types}
 		--version-file ${PX4_BINARY_DIR}/src/lib/version/build_git_version.h
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_crc.py
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_crc.py
 		${component_general_json}
 		${PX4_BINARY_DIR}/events/all_events.json.xz
 		--output ${component_information_header}
@@ -83,32 +83,3 @@ void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float dis

 	_distance_sensor_pub.update();
 }
-
-void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality,
-			    const float q[4])
-{
-
-	if (_distance_sensor_pub.get().orientation != distance_sensor_s::ROTATION_CUSTOM) {
-		PX4_ERR("Orientation not set to ROTATION_CUSTOM");
-		return;
-	}
-
-	distance_sensor_s &report = _distance_sensor_pub.get();
-
-	report.timestamp = timestamp_sample;
-	report.current_distance = distance;
-	report.signal_quality = quality;
-
-	// if quality is unavailable (-1) set to 0 if distance is outside bounds
-	if (quality < 0) {
-		if ((distance < report.min_distance) || (distance > report.max_distance)) {
-			report.signal_quality = 0;
-		}
-	}
-
-	for (int i = 0; i < 4; i++) {
-		report.q[i] = q[i];
-	}
-
-	_distance_sensor_pub.update();
-}
@@ -63,7 +63,6 @@ public:
 	void set_mode(const uint8_t mode) { _distance_sensor_pub.get().mode = mode; }

 	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);
-	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality, const float q[4]);

 	int get_instance() { return _distance_sensor_pub.get_instance(); };

@@ -47,7 +47,7 @@ set(generated_events_px4_file ${generated_events_dir}/px4.json)
 set(generated_events_common_enums_file ${generated_events_dir}/common_with_enums.json)
 add_custom_command(OUTPUT ${generated_events_px4_file}
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_events_dir}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py
 		--base-path ${PX4_SOURCE_DIR}/src
 		--src-path ${all_px4_src_files_relative}
 		--json ${generated_events_px4_file} #--verbose
@@ -64,13 +64,13 @@ add_custom_target(events_px4_json DEPENDS ${generated_events_px4_file})

 set(generated_events_file ${generated_events_dir}/all_events.json)
 add_custom_command(OUTPUT ${generated_events_file} ${generated_events_file}.xz
-	COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/combine.py
+	COMMAND ${Python_EXECUTABLE} libevents/scripts/combine.py
 		${generated_events_px4_file}
 		${generated_events_common_enums_file}
 		--output ${generated_events_file}
-	COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/validate.py
+	COMMAND ${Python_EXECUTABLE} libevents/scripts/validate.py
 		${generated_events_file}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py
 		${generated_events_file}
 	DEPENDS
 		${generated_events_px4_file}
@@ -86,7 +86,7 @@ add_custom_target(events_json DEPENDS ${generated_events_file})
 # combine common.json with our enums for the code generation
 add_custom_command(OUTPUT ${generated_events_common_enums_file}
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_events_dir}
-	COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/combine.py
+	COMMAND ${Python_EXECUTABLE} libevents/scripts/combine.py
 		enums.json
 		libevents/events/common.json
 		--output ${generated_events_common_enums_file}
@@ -105,7 +105,7 @@ add_custom_command(OUTPUT ${generated_events_common_enums_file}
 set(generated_events_header ${generated_events_dir}/events_generated.h)
 add_custom_command(OUTPUT ${generated_events_header}
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_events_dir}
-	COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/generate.py
+	COMMAND ${Python_EXECUTABLE} libevents/scripts/generate.py
 		--template libevents/libs/cpp/templates/events_generated.h.jinja
 		--output ${generated_events_dir}
 		${generated_events_common_enums_file}
@@ -55,16 +55,6 @@ public:
 		}
 	}

-	template<typename S>
-	Matrix(const Matrix<S, M, N> &aa)
-	{
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				_data[i][j] = static_cast<Type>(aa(i, j));
-			}
-		}
-	}
-
 	template<size_t P, size_t Q>
 	Matrix(const Slice<Type, M, N, P, Q> &in_slice)
 	{
@@ -39,14 +39,14 @@ endif()
 set(generated_actuators_metadata_file ${PX4_BINARY_DIR}/actuators.json)
 add_custom_command(OUTPUT ${generated_actuators_metadata_file}
 	${generated_actuators_metadata_file}.xz
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_actuators_metadata.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_actuators_metadata.py
 		${board_with_io_arg}
 		--timer-config ${PX4_BOARD_DIR}/src/timer_config.cpp
 		--config-files ${module_config_files} #--verbose
 		--compress
 		--board ${PX4_BOARD}
 		--output-file ${generated_actuators_metadata_file}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
 		--schema-file ${PX4_SOURCE_DIR}/src/modules/mavlink/mavlink/component_information/actuators.schema.json
 		${generated_actuators_metadata_file}
 		--skip-if-no-schema # mavlink submodule might not exist for current target if built in CI
@@ -33,7 +33,7 @@

 set(functions_header ${CMAKE_CURRENT_BINARY_DIR}/output_functions.hpp)
 add_custom_command(OUTPUT ${functions_header}
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_function_header.py
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_function_header.py
 		${CMAKE_CURRENT_SOURCE_DIR}/output_functions.yaml
 		${functions_header}
 	DEPENDS
@@ -106,32 +106,16 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const

 float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 {
-	const Vector3f &start_position = {_trajectory[0].getCurrentPosition(),
-					  _trajectory[1].getCurrentPosition(),
-					  _trajectory[2].getCurrentPosition()
-					 };
-	const Vector3f &target = waypoints[1];
-	const Vector3f &next_target = waypoints[2];

-	const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)};
-	const Vector2f target_xy_z = {target.xy().norm(), target(2)};
-	const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};
+	const auto &target = waypoints[1];

-	float arrival_z_speed = 0.0f;
-	const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;
+	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+			  _trajectory[1].getCurrentPosition(),
+			  _trajectory[2].getCurrentPosition());

-	if (target_next_different) {
-		const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot(
-						  Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero()));
+	const float distance_start_target = fabs(target(2) - pos_traj(2));
+	const float arrival_z_speed = 0.f;

-		const float safe_alpha = math::constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
-		float accel_tmp = _trajectory[2].getMaxAccel();
-		float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(safe_alpha, accel_tmp,
-					  _vertical_acceptance_radius);
-		arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel());
-	}
-
-	const float distance_start_target = fabs(target(2) - start_position(2));
 	float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
 					    _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
 					    distance_start_target, arrival_z_speed));
@@ -151,17 +151,14 @@ TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration)

 TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
 {
-	const int N_ITER = 20000;
+	const int N_ITER = 2000;
 	const float DELTA_T = 0.02f;
 	const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f};
 	const Vector3f TARGET{12.f, 17.f, 8.f};
 	const Vector3f NEXT_TARGET{8.f, 12.f, 80.f};

-	const float XY_ACC_RAD = 10.f;
-	const float Z_ACC_RAD = 0.8f;

-
-	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET};
+	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, NEXT_TARGET};
 	Vector3f ff_velocity{1.f, 0.1f, 0.3f};

 	Vector3f position{0.f, 0.f, 0.f};
@@ -183,13 +180,12 @@ TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
 		ff_velocity = {0.f, 0.f, 0.f};
 		expectDynamicsLimitsRespected(out);

-		if (Vector2f(position.xy() - TARGET.xy()).norm() < XY_ACC_RAD && fabsf(position(2) - TARGET(2)) < Z_ACC_RAD) {
+		if (position == TARGET) {
 			printf("Converged in %d iterations\n", iteration);
 			break;
 		}
 	}

-	EXPECT_LT(Vector2f(position.xy() - TARGET.xy()).norm(), XY_ACC_RAD);
-	EXPECT_LT(fabsf(position(2) - TARGET(2)), Z_ACC_RAD);
+	EXPECT_EQ(TARGET, position);
 	EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n";
 }
@@ -74,11 +74,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co

 	const bool target_next_different = distance_target_next  > 0.001f;
 	const bool waypoint_overlap = distance_target_next < config.xy_accept_rad;
+	const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad;
+	const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad;

 	float speed_at_target = 0.0f;

 	if (target_next_different &&
-	    !waypoint_overlap
+	    !waypoint_overlap &&
+	    has_reached_altitude &&
+	    altitude_stays_same
 	   ) {
 		const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot(
 						  Vector2f((target - next_target).xy()).unit_or_zero()));
@@ -104,15 +108,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
 *
 * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
 */
-template <int N>
+template <size_t N>
 float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
 {
 	static_assert(N >= 2, "Need at least 2 points to compute speed");

 	float max_speed = 0.f;

-	// go backwards through the waypoints
-	for (int i = (N - 2); i >= 0; i--) {
+	for (size_t j = 0; j < N - 1; j++) {
+		size_t i = N - 2 - j;
 		max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
 				waypoints[i + 1],
 				waypoints[min(i + 2, N - 1)],
@@ -90,12 +90,12 @@ endif()

 add_custom_command(OUTPUT ${generated_serial_params_file} ${generated_module_params_file}
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_params_dir}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
 		--params-file ${generated_serial_params_file}
 		--serial-ports ${board_serial_ports} ${added_arguments} ${constrained_flash_arg}
 		--config-files ${module_config_files}
 		#--verbose
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py
 		--params-file ${generated_module_params_file}
 		${added_arguments} ${board_with_io_arg}
 		--timer-config ${PX4_BOARD_DIR}/src/timer_config.cpp
@@ -114,7 +114,7 @@ set(parameters_xml ${PX4_BINARY_DIR}/parameters.xml)
 set(parameters_json ${PX4_BINARY_DIR}/parameters.json)
 file(GLOB_RECURSE param_src_files ${PX4_SOURCE_DIR}/src/*params.c ${PX4_SOURCE_DIR}/src/*parameters.c)
 add_custom_command(OUTPUT ${parameters_xml} ${parameters_json} ${parameters_json}.xz
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_process_params.py
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_process_params.py
 		--src-path ${module_list} ${generated_params_dir}
 		--xml ${parameters_xml}
 		--json ${parameters_json}
@@ -123,7 +123,7 @@ add_custom_command(OUTPUT ${parameters_xml} ${parameters_json} ${parameters_json
 		--overrides ${PARAM_DEFAULT_OVERRIDES}
 		--board ${PX4_BOARD}
 		#--verbose
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
 		--schema-file ${PX4_SOURCE_DIR}/src/modules/mavlink/mavlink/component_information/parameter.schema.json
 		${parameters_json}
 		--skip-if-no-schema # mavlink submodule might not exist for current target if built in CI
@@ -145,7 +145,7 @@ add_custom_target(parameters_xml DEPENDS ${parameters_xml})

 # generate px4_parameters.hpp
 add_custom_command(OUTPUT px4_parameters.hpp
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_generate_params.py
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_generate_params.py
 		--xml ${parameters_xml} --dest ${CMAKE_CURRENT_BINARY_DIR}
 	DEPENDS
 		${PX4_BINARY_DIR}/parameters.xml
@@ -157,7 +157,7 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp)
 set(SRCS)

 list(APPEND SRCS
-	parameters.cpp 
+	parameters.cpp
 	atomic_transaction.cpp
 	autosave.cpp
 )
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018 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,10 +31,4 @@
 #
 ############################################################################

-px4_add_library(PID
-	PID.cpp
-	PID.hpp
-)
-target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)
+px4_add_library(pid pid.cpp)
@@ -1,75 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022-2024 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 "PID.hpp"
-#include "lib/mathlib/math/Functions.hpp"
-
-void PID::setGains(const float P, const float I, const float D)
-{
-	_gain_proportional = P;
-	_gain_integral = I;
-	_gain_derivative = D;
-}
-
-float PID::update(const float feedback, const float dt, const bool update_integral)
-{
-	const float error = _setpoint - feedback;
-	const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt));
-
-	if (update_integral) {
-		updateIntegral(error, dt);
-	}
-
-	_last_feedback = feedback;
-	return math::constrain(output, -_limit_output, _limit_output);
-}
-
-void PID::updateIntegral(float error, const float dt)
-{
-	const float integral_new = _integral + _gain_integral * error * dt;
-
-	if (std::isfinite(integral_new)) {
-		_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
-	}
-}
-
-float PID::updateDerivative(float feedback, const float dt)
-{
-	float feedback_change = 0.f;
-
-	if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) {
-		feedback_change = (feedback - _last_feedback) / dt;
-	}
-
-	return feedback_change;
-}
@@ -1,65 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <cmath>
-
-class PID
-{
-public:
-	PID() = default;
-	virtual ~PID() = default;
-	void setOutputLimit(const float limit) { _limit_output = limit; }
-	void setIntegralLimit(const float limit) { _limit_integral = limit; }
-	void setGains(const float P, const float I, const float D);
-	void setSetpoint(const float setpoint) { _setpoint = setpoint; }
-	float update(const float feedback, const float dt, const bool update_integral = true);
-	float getIntegral() { return _integral; }
-	void resetIntegral() { _integral = 0.f; };
-	void resetDerivative() { _last_feedback = NAN; };
-private:
-	void updateIntegral(float error, const float dt);
-	float updateDerivative(float feedback, const float dt);
-
-	float _setpoint{0.f}; ///< current setpoint to track
-	float _integral{0.f}; ///< integral state
-	float _last_feedback{NAN};
-
-	// Gains, Limits
-	float _gain_proportional{0.f};
-	float _gain_integral{0.f};
-	float _gain_derivative{0.f};
-	float _limit_integral{0.f};
-	float _limit_output{0.f};
-};
@@ -1,130 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2022 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 <gtest/gtest.h>
-#include <PID.hpp>
-
-TEST(PIDTest, AllZeroCase)
-{
-	PID pid;
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
-}
-
-TEST(PIDTest, OutputLimit)
-{
-	PID pid;
-	pid.setOutputLimit(.01f);
-	pid.setGains(.1f, 0.f, 0.f);
-	pid.setSetpoint(1.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
-	EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
-	EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
-	EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
-	EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
-	EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
-	EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
-}
-
-TEST(PIDTest, ProportinalOnly)
-{
-	PID pid;
-	pid.setOutputLimit(1.f);
-	pid.setGains(.1f, 0.f, 0.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
-	pid.setSetpoint(1.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
-
-	float plant = 0.f;
-	float output = 10000.f;
-	int i; // need function scope to check how many steps
-
-	for (i = 1000; i > 0; i--) {
-		const float output_new = pid.update(plant, 0.f, false);
-		plant += output_new;
-
-		// expect the output to get smaller with each iteration
-		if (output_new >= output) {
-			break;
-		}
-
-		output = output_new;
-	}
-
-	EXPECT_FLOAT_EQ(plant, 1.f);
-	EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
-}
-
-TEST(PIDTest, InteralOpenLoop)
-{
-	PID pid;
-	pid.setOutputLimit(1.f);
-	pid.setGains(0.f, .1f, 0.f);
-	pid.setIntegralLimit(.05f);
-	pid.setSetpoint(1.f);
-
-	// Zero error
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
-
-	// Open loop ramp up
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
-
-	// Open loop ramp down
-	pid.setSetpoint(-1.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
-	EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
-	EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
-	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
-	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
-	pid.resetIntegral();
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
-}
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 pid.cpp
+ *
+ * Implementation of generic PID controller.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include "pid.h"
+#include <math.h>
+#include <px4_platform_common/defines.h>
+
+#define SIGMA 0.000001f
+
+__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
+{
+	pid->mode = mode;
+	pid->dt_min = dt_min;
+	pid->kp = 0.0f;
+	pid->ki = 0.0f;
+	pid->kd = 0.0f;
+	pid->integral = 0.0f;
+	pid->integral_limit = 0.0f;
+	pid->output_limit = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->last_output = 0.0f;
+}
+
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
+{
+	int ret = 0;
+
+	if (PX4_ISFINITE(kp)) {
+		pid->kp = kp;
+
+	} else {
+		ret = 1;
+	}
+
+	if (PX4_ISFINITE(ki)) {
+		pid->ki = ki;
+
+	} else {
+		ret = 1;
+	}
+
+	if (PX4_ISFINITE(kd)) {
+		pid->kd = kd;
+
+	} else {
+		ret = 1;
+	}
+
+	if (PX4_ISFINITE(integral_limit)) {
+		pid->integral_limit = integral_limit;
+
+	}  else {
+		ret = 1;
+	}
+
+	if (PX4_ISFINITE(output_limit)) {
+		pid->output_limit = output_limit;
+
+	}  else {
+		ret = 1;
+	}
+
+	return ret;
+}
+
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
+{
+	if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
+		return pid->last_output;
+	}
+
+	float i, d;
+
+	/* current error value */
+	float error = sp - val;
+
+	/* current error derivative */
+	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
+		d = -val_dot;
+
+	} else {
+		d = 0.0f;
+	}
+
+	if (!PX4_ISFINITE(d)) {
+		d = 0.0f;
+	}
+
+	/* calculate PD output */
+	float output = (error * pid->kp) + (d * pid->kd);
+
+	if (pid->ki > SIGMA) {
+		// Calculate the error integral and check for saturation
+		i = pid->integral + (error * dt);
+
+		/* check for saturation */
+		if (PX4_ISFINITE(i)) {
+			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
+			    fabsf(i) <= pid->integral_limit) {
+				/* not saturated, use new integral value */
+				pid->integral = i;
+			}
+		}
+
+		/* add I component to output */
+		output += pid->integral * pid->ki;
+	}
+
+	/* limit output */
+	if (PX4_ISFINITE(output)) {
+		if (pid->output_limit > SIGMA) {
+			if (output > pid->output_limit) {
+				output = pid->output_limit;
+
+			} else if (output < -pid->output_limit) {
+				output = -pid->output_limit;
+			}
+		}
+
+		pid->last_output = output;
+	}
+
+	return pid->last_output;
+}
+
+
+__EXPORT void pid_reset_integral(PID_t *pid)
+{
+	pid->integral = 0.0f;
+}
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 pid.h
+ *
+ * Definition of generic PID controller.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+typedef enum PID_MODE {
+	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
+	PID_MODE_DERIVATIV_NONE = 0,
+	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
+	 * val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC,
+	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
+	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC_NO_SP,
+	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
+	PID_MODE_DERIVATIV_SET
+} pid_mode_t;
+
+typedef struct {
+	pid_mode_t mode;
+	float dt_min;
+	float kp;
+	float ki;
+	float kd;
+	float integral;
+	float integral_limit;
+	float output_limit;
+	float error_previous;
+	float last_output;
+} PID_t;
+
+__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
+__EXPORT void pid_reset_integral(PID_t *pid);
+
+__END_DECLS
+
+#endif /* PID_H_ */
@@ -54,7 +54,7 @@ endif()
 set(px4_git_ver_header ${CMAKE_CURRENT_BINARY_DIR}/build_git_version.h)
 add_custom_command(OUTPUT ${px4_git_ver_header}
 	COMMAND
-		${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate --git_tag '${PX4_GIT_TAG}'
+		${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate --git_tag '${PX4_GIT_TAG}'
 	DEPENDS
 		${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py
 		${git_dir_path}/HEAD
@@ -40,7 +40,7 @@ add_dependencies(wind_estimator prebuild_targets)
 add_subdirectory(test)

 add_custom_target(wind_estimator_generate_airspeed_fusion
-	COMMAND ${PYTHON_EXECUTABLE} derivation.py
+	COMMAND ${Python_EXECUTABLE} derivation.py
 	#DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/python/derivation.py
 	#BYPRODUCTS ${CMAKE_CURRENT_SOURCE_DIR}/python/generated/fuse_airspeed.h
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/python
@@ -32,7 +32,7 @@
 ############################################################################

 add_custom_target(world_magnetic_model_update
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py > ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py > ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
 	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	USES_TERMINAL
@@ -40,7 +40,7 @@ add_custom_target(world_magnetic_model_update
 )

 add_custom_target(world_magnetic_model_tests_update
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
 	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	USES_TERMINAL
@@ -77,7 +77,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)

 	esc_status_s esc_status;

-	if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + esc_telemetry_timeout)) {
+	if (_esc_status_sub.copy(&esc_status) && now - esc_status.timestamp < esc_telemetry_timeout) {

 		checkEscStatus(context, reporter, esc_status);
 		reporter.setIsPresent(health_component_t::motors_escs);
@@ -813,7 +813,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f


 	// altitude
-	failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + 1_s);
+	failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + (_param_com_pos_fs_delay.get() * 1_s));


 	// attitude
@@ -864,7 +864,7 @@ bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool dat
 		const bool was_valid) const
 {
 	bool valid = was_valid;
-	const bool data_stale = (now > data_timestamp_us + 1_s) || (data_timestamp_us == 0);
+	const bool data_stale = (now > data_timestamp_us + _param_com_pos_fs_delay.get() * 1_s) || (data_timestamp_us == 0);
 	const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
 	const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);

@@ -118,6 +118,7 @@ private:
 					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
+					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
 					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph,
 					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
 				       )
@@ -512,6 +512,19 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0);
 */
 PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);

+/**
+ * Loss of position failsafe activation delay.
+ *
+ * This sets number of seconds that the position checks need to be failed before the failsafe will activate.
+ * The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5 and 10 should be used.
+ *
+ * @unit s
+ * @group Commander
+ * @min 1
+ * @max 100
+ */
+PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1);
+
 /**
 * Horizontal position error threshold.
 *
@@ -37,7 +37,7 @@ set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_
 set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html)
 set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html)
 add_custom_command(OUTPUT ${generated_uorb_struct_field_mapping_header} ${html_output_file}
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
 		${failsafe_flags_msg_file} ${generated_uorb_struct_field_mapping_header}
 		${html_template_file} ${html_output_file}
 	DEPENDS
@@ -35,7 +35,7 @@ option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF)

 # Symforce code generation TODO:fixme
 execute_process(
-    COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
+    COMMAND ${Python_EXECUTABLE} -m symforce.symbolic
    RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE
    OUTPUT_QUIET
 )
@@ -58,7 +58,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
 			${EKF_DERIVATION_SRC_DIR}/generated/predict_covariance.h
 			${EKF_DERIVATION_SRC_DIR}/generated/state.h
 		COMMAND
-			${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py
+			${Python_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py
 		DEPENDS
 			${EKF_DERIVATION_SRC_DIR}/derivation.py
 			${EKF_DERIVATION_SRC_DIR}/utils/derivation_utils.py
@@ -92,7 +92,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
 			${EKF_DERIVATION_DST_DIR}/generated/predict_covariance.h
 			${EKF_DERIVATION_DST_DIR}/generated/state.h
 		COMMAND
-			${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS}
+			${Python_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS}
 		DEPENDS
 			${EKF_DERIVATION_SRC_DIR}/derivation.py
 			${EKF_DERIVATION_SRC_DIR}/utils/derivation_utils.py
@@ -33,6 +33,7 @@

 add_subdirectory(bias_estimator)
 add_subdirectory(output_predictor)
+add_subdirectory(lat_lon_alt)

 set(EKF_LIBS)
 set(EKF_SRCS)
@@ -110,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed

 				} else {
 					// Try to initialize using measurement
-					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
-								      sq(sample.epv))) {
+					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
+								      sample.epv)) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;
@@ -66,6 +66,8 @@
 # include "aid_sources/aux_global_position/aux_global_position.hpp"
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION

+#include "lat_lon_alt/lat_lon_alt.hpp"
+
 enum class Likelihood { LOW, MEDIUM, HIGH };
 class ExternalVisionVel;

@@ -191,9 +193,9 @@ public:
 	void getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
 	bool checkLatLonValidity(double latitude, double longitude);
 	bool checkAltitudeValidity(float altitude);
-	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float hpos_var = NAN, float vpos_var = NAN);
-	bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float hpos_var = NAN,
-				   float vpos_var = NAN);
+	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
+	bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN,
+				   float epv = NAN);

 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@@ -42,6 +42,7 @@
 #ifndef EKF_ESTIMATOR_INTERFACE_H
 #define EKF_ESTIMATOR_INTERFACE_H

+#include "lat_lon_alt/lat_lon_alt.hpp"
 #if defined(MODULE_NAME)
 #include <px4_platform_common/log.h>
 # define ECL_INFO PX4_DEBUG
@@ -72,7 +73,6 @@
 #endif // CONFIG_EKF2_RANGE_FINDER

 #include <lib/atmosphere/atmosphere.h>
-#include <lib/lat_lon_alt/lat_lon_alt.hpp>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/AlphaFilter.hpp>
@@ -34,52 +34,8 @@
 #include "lat_lon_alt.hpp"

 using matrix::Vector3f;
-using matrix::Vector3d;
 using matrix::Vector2d;

-LatLonAlt LatLonAlt::fromEcef(const Vector3d &p_ecef)
-{
-	// Convert position using Borkowski closed-form exact solution
-	// P. D. Groves, "Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd edition (appendix C)
-	const double k1 = sqrt(1 - Wgs84::eccentricity2) * std::abs(p_ecef(2));
-	const double k2 = Wgs84::eccentricity2 * Wgs84::equatorial_radius;
-	const double beta = sqrt(p_ecef(0) * p_ecef(0) + p_ecef(1) * p_ecef(1));
-	const double E = (k1 - k2) / beta;
-	const double F = (k1 + k2) / beta;
-
-	const double P = 4.0 / 3.0 * (E * F + 1);
-	const double Q = 2 * (E * E - F * F);
-	const double D = P * P * P + Q * Q;
-	const double V = pow(sqrt(D) - Q, 1.0 / 3.0) - pow(sqrt(D) + Q, 1.0 / 3.0);
-	const double G = 0.5 * (sqrt(E * E + V) + E);
-	const double T = sqrt(G * G + (F - V * G) / (2 * G - E)) - G;
-
-	const double lon = atan2(p_ecef(1), p_ecef(0));
-	const double lat = matrix::sign(p_ecef(2)) * atan((1.0 - T * T) / (2.0 * T * sqrt(1.0 - Wgs84::eccentricity2)));
-	const double alt = (beta - Wgs84::equatorial_radius * T) * cos(lat) +
-			   (p_ecef(2) - matrix::sign(p_ecef(2)) * Wgs84::equatorial_radius * sqrt(1.0 - Wgs84::eccentricity2)) * sin(lat);
-
-	LatLonAlt lla;
-	lla.setLatLonRad(lat, lon);
-	lla.setAltitude(static_cast<float>(alt));
-	return lla;
-}
-
-Vector3d LatLonAlt::toEcef() const
-{
-	const double cos_lat = cos(_latitude_rad);
-	const double sin_lat = sin(_latitude_rad);
-	const double cos_lon = cos(_longitude_rad);
-	const double sin_lon = sin(_longitude_rad);
-
-	const double r_e = Wgs84::equatorial_radius / sqrt(1.0 - std::pow(Wgs84::eccentricity * sin_lat, 2.0));
-	const double r_total = r_e + static_cast<double>(_altitude);
-
-	return Vector3d(r_total * cos_lat * cos_lon,
-			r_total * cos_lat * sin_lon,
-			((1.0 - Wgs84::eccentricity2) * r_e + static_cast<double>(_altitude)) * sin_lat);
-}
-
 Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const
 {
 	double r_n;
@@ -54,10 +54,6 @@ public:
 		_altitude = altitude_m;
 	}

-
-	static LatLonAlt fromEcef(const matrix::Vector3d &p_ecef);
-	matrix::Vector3d toEcef() const;
-
 	void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; }

 	double latitude_deg() const { return math::degrees(latitude_rad()); }
@@ -98,14 +94,6 @@ public:
 	 */
 	matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) const;

-	struct Wgs84 {
-		static constexpr double equatorial_radius = 6378137.0;
-		static constexpr double eccentricity = 0.0818191908425;
-		static constexpr double eccentricity2 = eccentricity * eccentricity;
-		static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity2);
-		static constexpr double gravity_equator = 9.7803253359;
-	};
-
 private:
 	// Convert between curvilinear and cartesian errors
 	static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude);
@@ -113,6 +101,12 @@ private:
 	static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
 					    double &transverse_radius_of_curvature);

+	struct Wgs84 {
+		static constexpr double equatorial_radius = 6378137.0;
+		static constexpr double eccentricity = 0.0818191908425;
+		static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity *eccentricity);
+	};
+
 	double _latitude_rad{0.0};
 	double _longitude_rad{0.0};
 	float _altitude{0.0};
@@ -105,17 +105,3 @@ TEST(TestLatLonAlt, subLatLonAlt)
 	EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2);
 	EXPECT_EQ(delta_pos(2), delta_pos_true(2));
 }
-
-TEST(TestLatLonAlt, fromAndToECEF)
-{
-	for (double lat = -M_PI; lat < M_PI; lat += M_PI / 4.0) {
-		for (double lon = -M_PI; lon < M_PI; lon += M_PI / 4.0) {
-			for (float alt = -500.f; alt < 8000.f; alt += 500.f) {
-				LatLonAlt lla(lat, lon, alt);
-
-				LatLonAlt res = LatLonAlt::fromEcef(lla.toEcef());
-				EXPECT_TRUE(!(lla - res).longerThan(10e-6f)) << "lat: " << lat << ", lon: " << lon << ", alt: " << alt;
-			}
-		}
-	}
-}
@@ -37,9 +37,9 @@
 #include <matrix/math.hpp>

 #include "../RingBuffer.h"
+#include "../lat_lon_alt/lat_lon_alt.hpp"

 #include <lib/geo/geo.h>
-#include <lib/lat_lon_alt/lat_lon_alt.hpp>

 class OutputPredictor
 {
@@ -337,7 +337,9 @@ def predict_sideslip(
    vel_rel = state["vel"] - wind
    relative_wind_body = state["quat_nominal"].inverse() * vel_rel

-    sideslip_pred = sf.atan2(relative_wind_body[1], relative_wind_body[0], epsilon)
+    # Small angle approximation of side slip model
+    # Protect division by zero using epsilon
+    sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon)

    return sideslip_pred

@@ -30,65 +30,66 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 518
+  // Total ops: 513

  // Input arrays

-  // Intermediate terms (50)
-  const Scalar _tmp0 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp1 = 2 * _tmp0;
-  const Scalar _tmp2 = 2 * state(3, 0);
-  const Scalar _tmp3 = _tmp2 * state(6, 0);
-  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp6 = _tmp2 * state(0, 0);
-  const Scalar _tmp7 = 2 * state(1, 0);
-  const Scalar _tmp8 = _tmp7 * state(2, 0);
-  const Scalar _tmp9 = _tmp6 + _tmp8;
-  const Scalar _tmp10 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp11 = 2 * state(2, 0);
-  const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0);
-  const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0);
-  const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
-  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
-  const Scalar _tmp16 = 2 * _tmp10;
-  const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0);
-  const Scalar _tmp18 = std::pow(_tmp14, Scalar(2));
-  const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp20 = -_tmp6 + _tmp8;
-  const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0);
-  const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0);
-  const Scalar _tmp23 = _tmp22 / _tmp18;
-  const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) -
-                        _tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17);
-  const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2)));
-  const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25;
-  const Scalar _tmp27 = _tmp26 * state(3, 0);
-  const Scalar _tmp28 = _tmp7 * state(6, 0);
-  const Scalar _tmp29 = _tmp11 * state(6, 0);
+  // Intermediate terms (43)
+  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp3 = 4 * _tmp2;
+  const Scalar _tmp4 = 2 * state(6, 0);
+  const Scalar _tmp5 = _tmp4 * state(0, 0);
+  const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp8 = 2 * state(0, 0);
+  const Scalar _tmp9 = _tmp8 * state(3, 0);
+  const Scalar _tmp10 = 2 * state(2, 0);
+  const Scalar _tmp11 = _tmp10 * state(1, 0);
+  const Scalar _tmp12 = _tmp11 + _tmp9;
+  const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0);
+  const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7;
+  const Scalar _tmp15 =
+      _tmp14 + epsilon * (2 * math::min<Scalar>(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1);
+  const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp17 = _tmp11 - _tmp9;
+  const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0);
+  const Scalar _tmp19 =
+      (_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2));
+  const Scalar _tmp20 = _tmp4 * state(3, 0);
+  const Scalar _tmp21 = Scalar(1.0) / (_tmp15);
+  const Scalar _tmp22 =
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) +
+      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20);
+  const Scalar _tmp23 = 4 * _tmp0;
+  const Scalar _tmp24 =
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) +
+      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5);
+  const Scalar _tmp25 = 2 * state(3, 0);
+  const Scalar _tmp26 = _tmp4 * state(2, 0);
+  const Scalar _tmp27 = _tmp4 * state(1, 0);
+  const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) +
+                        (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27);
+  const Scalar _tmp29 =
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) +
+      (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26);
  const Scalar _tmp30 =
-      _tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29);
-  const Scalar _tmp31 = _tmp26 * state(1, 0);
-  const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) -
-                        _tmp23 * (_tmp16 * state(2, 0) + _tmp3);
-  const Scalar _tmp33 = _tmp26 * state(0, 0);
-  const Scalar _tmp34 = 4 * state(3, 0);
-  const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) -
-                        _tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28);
-  const Scalar _tmp36 = _tmp26 * state(2, 0);
-  const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36;
-  const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35;
-  const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35;
-  const Scalar _tmp40 = _tmp23 * _tmp5;
-  const Scalar _tmp41 = _tmp15 * _tmp20;
-  const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41);
-  const Scalar _tmp43 = _tmp15 * _tmp19;
-  const Scalar _tmp44 = _tmp23 * _tmp9;
-  const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44);
-  const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21);
-  const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41);
-  const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44);
-  const Scalar _tmp49 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
+      -_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0);
+  const Scalar _tmp31 =
+      _tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0);
+  const Scalar _tmp32 =
+      _tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0);
+  const Scalar _tmp33 = _tmp19 * _tmp7;
+  const Scalar _tmp34 = _tmp17 * _tmp21;
+  const Scalar _tmp35 = -_tmp33 + _tmp34;
+  const Scalar _tmp36 = _tmp12 * _tmp19;
+  const Scalar _tmp37 = _tmp16 * _tmp21;
+  const Scalar _tmp38 = -_tmp36 + _tmp37;
+  const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21;
+  const Scalar _tmp40 = _tmp33 - _tmp34;
+  const Scalar _tmp41 = _tmp36 - _tmp37;
+  const Scalar _tmp42 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));

  // Output terms (2)
  if (H != nullptr) {
@@ -96,91 +97,91 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,

    _h.setZero();

-    _h(0, 0) = _tmp37;
-    _h(1, 0) = _tmp38;
-    _h(2, 0) = _tmp39;
-    _h(3, 0) = _tmp42;
-    _h(4, 0) = _tmp45;
-    _h(5, 0) = _tmp46;
-    _h(21, 0) = _tmp47;
-    _h(22, 0) = _tmp48;
+    _h(0, 0) = _tmp30;
+    _h(1, 0) = _tmp31;
+    _h(2, 0) = _tmp32;
+    _h(3, 0) = _tmp35;
+    _h(4, 0) = _tmp38;
+    _h(5, 0) = _tmp39;
+    _h(21, 0) = _tmp40;
+    _h(22, 0) = _tmp41;
  }

  if (K != nullptr) {
    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

    _k(0, 0) =
-        _tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 +
-                  P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46);
+        _tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 +
+                  P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39);
    _k(1, 0) =
-        _tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 +
-                  P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46);
+        _tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 +
+                  P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39);
    _k(2, 0) =
-        _tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 +
-                  P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46);
+        _tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 +
+                  P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39);
    _k(3, 0) =
-        _tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 +
-                  P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46);
+        _tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 +
+                  P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39);
    _k(4, 0) =
-        _tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 +
-                  P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46);
+        _tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 +
+                  P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39);
    _k(5, 0) =
-        _tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 +
-                  P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46);
+        _tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 +
+                  P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39);
    _k(6, 0) =
-        _tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 +
-                  P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46);
+        _tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 +
+                  P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39);
    _k(7, 0) =
-        _tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 +
-                  P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46);
+        _tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 +
+                  P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39);
    _k(8, 0) =
-        _tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 +
-                  P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46);
+        _tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 +
+                  P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39);
    _k(9, 0) =
-        _tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 +
-                  P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46);
+        _tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 +
+                  P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39);
    _k(10, 0) =
-        _tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 +
-                  P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46);
+        _tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 +
+                  P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39);
    _k(11, 0) =
-        _tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 +
-                  P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46);
+        _tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 +
+                  P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39);
    _k(12, 0) =
-        _tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 +
-                  P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46);
+        _tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 +
+                  P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39);
    _k(13, 0) =
-        _tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 +
-                  P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46);
+        _tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 +
+                  P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39);
    _k(14, 0) =
-        _tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 +
-                  P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46);
+        _tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 +
+                  P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39);
    _k(15, 0) =
-        _tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 +
-                  P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46);
+        _tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 +
+                  P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39);
    _k(16, 0) =
-        _tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 +
-                  P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46);
+        _tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 +
+                  P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39);
    _k(17, 0) =
-        _tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 +
-                  P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46);
+        _tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 +
+                  P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39);
    _k(18, 0) =
-        _tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 +
-                  P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46);
+        _tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 +
+                  P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39);
    _k(19, 0) =
-        _tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 +
-                  P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46);
+        _tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 +
+                  P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39);
    _k(20, 0) =
-        _tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 +
-                  P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46);
+        _tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 +
+                  P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39);
    _k(21, 0) =
-        _tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 +
-                  P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46);
+        _tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 +
+                  P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39);
    _k(22, 0) =
-        _tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 +
-                  P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46);
+        _tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 +
+                  P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39);
    _k(23, 0) =
-        _tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 +
-                  P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46);
+        _tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 +
+                  P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39);
  }
 }  // NOLINT(readability/fn_size)

@@ -30,69 +30,70 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                     const Scalar epsilon, Scalar* const innov = nullptr,
                                     Scalar* const innov_var = nullptr) {
-  // Total ops: 266
+  // Total ops: 265

  // Input arrays

-  // Intermediate terms (49)
+  // Intermediate terms (42)
  const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp3 = 2 * state(3, 0);
-  const Scalar _tmp4 = _tmp3 * state(0, 0);
+  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp3 = 2 * state(0, 0);
+  const Scalar _tmp4 = _tmp3 * state(3, 0);
  const Scalar _tmp5 = 2 * state(1, 0);
  const Scalar _tmp6 = _tmp5 * state(2, 0);
-  const Scalar _tmp7 = -_tmp4 + _tmp6;
-  const Scalar _tmp8 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp7 = _tmp4 + _tmp6;
+  const Scalar _tmp8 = -state(23, 0) + state(5, 0);
  const Scalar _tmp9 = 2 * state(2, 0);
-  const Scalar _tmp10 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
+  const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
  const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
-  const Scalar _tmp12 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp13 = _tmp4 + _tmp6;
-  const Scalar _tmp14 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
-  const Scalar _tmp15 = _tmp12 * _tmp8 + _tmp13 * _tmp2 + _tmp14 * state(6, 0);
-  const Scalar _tmp16 = _tmp15 + epsilon * ((((_tmp15) > 0) - ((_tmp15) < 0)) + Scalar(0.5));
-  const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
-  const Scalar _tmp18 = _tmp1 * _tmp17;
-  const Scalar _tmp19 = std::pow(_tmp16, Scalar(2));
-  const Scalar _tmp20 = _tmp11 / _tmp19;
-  const Scalar _tmp21 = _tmp13 * _tmp20;
-  const Scalar _tmp22 = _tmp19 / (std::pow(_tmp11, Scalar(2)) + _tmp19);
-  const Scalar _tmp23 = _tmp22 * (-_tmp18 + _tmp21);
-  const Scalar _tmp24 = _tmp12 * _tmp20;
-  const Scalar _tmp25 = _tmp17 * _tmp7;
-  const Scalar _tmp26 = _tmp22 * (-_tmp24 + _tmp25);
-  const Scalar _tmp27 = _tmp22 * (_tmp10 * _tmp17 - _tmp14 * _tmp20);
-  const Scalar _tmp28 = _tmp3 * state(6, 0);
-  const Scalar _tmp29 = 4 * _tmp8;
-  const Scalar _tmp30 = 2 * state(0, 0);
-  const Scalar _tmp31 = _tmp30 * state(6, 0);
-  const Scalar _tmp32 =
-      _tmp17 * (_tmp28 + _tmp5 * _tmp8) - _tmp20 * (_tmp2 * _tmp5 - _tmp29 * state(2, 0) - _tmp31);
-  const Scalar _tmp33 = (Scalar(1) / Scalar(2)) * _tmp22;
-  const Scalar _tmp34 = _tmp33 * state(1, 0);
-  const Scalar _tmp35 = _tmp5 * state(6, 0);
-  const Scalar _tmp36 = _tmp9 * state(6, 0);
-  const Scalar _tmp37 = _tmp17 * (-_tmp3 * _tmp8 + _tmp35) - _tmp20 * (_tmp2 * _tmp3 - _tmp36);
-  const Scalar _tmp38 = _tmp33 * state(3, 0);
-  const Scalar _tmp39 = 4 * _tmp2;
-  const Scalar _tmp40 =
-      _tmp17 * (_tmp31 - _tmp39 * state(1, 0) + _tmp8 * _tmp9) - _tmp20 * (_tmp2 * _tmp9 + _tmp28);
-  const Scalar _tmp41 = _tmp33 * state(2, 0);
-  const Scalar _tmp42 = _tmp17 * (-_tmp30 * _tmp8 + _tmp36 - _tmp39 * state(3, 0)) -
-                        _tmp20 * (_tmp2 * _tmp30 - _tmp29 * state(3, 0) + _tmp35);
-  const Scalar _tmp43 = _tmp33 * state(0, 0);
-  const Scalar _tmp44 = _tmp32 * _tmp34 - _tmp37 * _tmp38 - _tmp40 * _tmp41 + _tmp42 * _tmp43;
-  const Scalar _tmp45 = -_tmp32 * _tmp38 - _tmp34 * _tmp37 + _tmp40 * _tmp43 + _tmp41 * _tmp42;
-  const Scalar _tmp46 = _tmp32 * _tmp43 - _tmp34 * _tmp42 - _tmp37 * _tmp41 + _tmp38 * _tmp40;
-  const Scalar _tmp47 = _tmp22 * (_tmp24 - _tmp25);
-  const Scalar _tmp48 = _tmp22 * (_tmp18 - _tmp21);
+  const Scalar _tmp12 =
+      _tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
+  const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
+  const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp15 = -_tmp4 + _tmp6;
+  const Scalar _tmp16 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
+  const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
+  const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
+  const Scalar _tmp19 = _tmp18 * _tmp7;
+  const Scalar _tmp20 = _tmp13 * _tmp14;
+  const Scalar _tmp21 = _tmp19 - _tmp20;
+  const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
+  const Scalar _tmp23 = _tmp1 * _tmp18;
+  const Scalar _tmp24 = _tmp13 * _tmp15;
+  const Scalar _tmp25 = -_tmp23 + _tmp24;
+  const Scalar _tmp26 = 2 * state(6, 0);
+  const Scalar _tmp27 = _tmp26 * state(0, 0);
+  const Scalar _tmp28 = _tmp26 * state(3, 0);
+  const Scalar _tmp29 =
+      (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) -
+      Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8);
+  const Scalar _tmp30 =
+      (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) -
+      Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9);
+  const Scalar _tmp31 = 2 * state(3, 0);
+  const Scalar _tmp32 = _tmp26 * state(2, 0);
+  const Scalar _tmp33 = _tmp26 * state(1, 0);
+  const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) -
+                        Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32);
+  const Scalar _tmp35 = 4 * state(3, 0);
+  const Scalar _tmp36 =
+      (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) -
+      Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33);
+  const Scalar _tmp37 =
+      _tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0);
+  const Scalar _tmp38 =
+      -_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0);
+  const Scalar _tmp39 =
+      _tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0);
+  const Scalar _tmp40 = _tmp23 - _tmp24;
+  const Scalar _tmp41 = -_tmp19 + _tmp20;

  // Output terms (2)
  if (innov != nullptr) {
    Scalar& _innov = (*innov);

-    _innov = std::atan2(_tmp11, _tmp16);
+    _innov = _tmp13 * _tmp17;
  }

  if (innov_var != nullptr) {
@@ -100,22 +101,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,

    _innov_var =
        R +
-        _tmp23 * (P(0, 22) * _tmp45 + P(1, 22) * _tmp46 + P(2, 22) * _tmp44 + P(21, 22) * _tmp47 +
-                  P(22, 22) * _tmp23 + P(3, 22) * _tmp26 + P(4, 22) * _tmp48 + P(5, 22) * _tmp27) +
-        _tmp26 * (P(0, 3) * _tmp45 + P(1, 3) * _tmp46 + P(2, 3) * _tmp44 + P(21, 3) * _tmp47 +
-                  P(22, 3) * _tmp23 + P(3, 3) * _tmp26 + P(4, 3) * _tmp48 + P(5, 3) * _tmp27) +
-        _tmp27 * (P(0, 5) * _tmp45 + P(1, 5) * _tmp46 + P(2, 5) * _tmp44 + P(21, 5) * _tmp47 +
-                  P(22, 5) * _tmp23 + P(3, 5) * _tmp26 + P(4, 5) * _tmp48 + P(5, 5) * _tmp27) +
-        _tmp44 * (P(0, 2) * _tmp45 + P(1, 2) * _tmp46 + P(2, 2) * _tmp44 + P(21, 2) * _tmp47 +
-                  P(22, 2) * _tmp23 + P(3, 2) * _tmp26 + P(4, 2) * _tmp48 + P(5, 2) * _tmp27) +
-        _tmp45 * (P(0, 0) * _tmp45 + P(1, 0) * _tmp46 + P(2, 0) * _tmp44 + P(21, 0) * _tmp47 +
-                  P(22, 0) * _tmp23 + P(3, 0) * _tmp26 + P(4, 0) * _tmp48 + P(5, 0) * _tmp27) +
-        _tmp46 * (P(0, 1) * _tmp45 + P(1, 1) * _tmp46 + P(2, 1) * _tmp44 + P(21, 1) * _tmp47 +
-                  P(22, 1) * _tmp23 + P(3, 1) * _tmp26 + P(4, 1) * _tmp48 + P(5, 1) * _tmp27) +
-        _tmp47 * (P(0, 21) * _tmp45 + P(1, 21) * _tmp46 + P(2, 21) * _tmp44 + P(21, 21) * _tmp47 +
-                  P(22, 21) * _tmp23 + P(3, 21) * _tmp26 + P(4, 21) * _tmp48 + P(5, 21) * _tmp27) +
-        _tmp48 * (P(0, 4) * _tmp45 + P(1, 4) * _tmp46 + P(2, 4) * _tmp44 + P(21, 4) * _tmp47 +
-                  P(22, 4) * _tmp23 + P(3, 4) * _tmp26 + P(4, 4) * _tmp48 + P(5, 4) * _tmp27);
+        _tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 +
+                  P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) +
+        _tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 +
+                  P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) +
+        _tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 +
+                  P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) +
+        _tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 +
+                  P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) +
+        _tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 +
+                  P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) +
+        _tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 +
+                  P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) +
+        _tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 +
+                  P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) +
+        _tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 +
+                  P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22);
  }
 }  // NOLINT(readability/fn_size)

@@ -383,7 +383,7 @@ TEST_F(EkfFlowTest, deadReckoning)
 	const float altitude_new  = 1500.0;
 	const float eph = 50.f;
 	const float epv = 10.f;
-	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph * eph, epv * epv);
+	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph, epv);

 	const Vector3f lpos_after_reset = _ekf->getPosition();

@@ -76,7 +76,7 @@ add_custom_command(
 	OUTPUT
 		${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp
 		${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp
-	COMMAND ${PYTHON_EXECUTABLE} generate_flight_tasks.py ${python_args}
+	COMMAND ${Python_EXECUTABLE} generate_flight_tasks.py ${python_args}
 	COMMENT "Generating Flight Tasks"
 	DEPENDS
 		Templates/FlightTasks_generated.cpp.em
@@ -323,9 +323,7 @@ void LoggedTopics::add_sensor_comparison_topics()
 void LoggedTopics::add_vision_and_avoidance_topics()
 {
 	add_topic("collision_constraints");
-	add_topic_multi("distance_sensor");
 	add_topic("obstacle_distance_fused");
-	add_topic("obstacle_distance");
 	add_topic("vehicle_mocap_odometry", 30);
 	add_topic("vehicle_trajectory_waypoint", 200);
 	add_topic("vehicle_trajectory_waypoint_desired", 200);
@@ -42,7 +42,7 @@ set(MAVLINK_DIALECT_UAVIONIX "uAvionix")
 add_custom_command(
 	OUTPUT ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT_UAVIONIX}/${MAVLINK_DIALECT_UAVIONIX}.h
 	COMMAND
-		${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
+		${Python_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
 			--lang C --wire-protocol 2.0
 			--output ${MAVLINK_LIBRARY_DIR}
 			${MAVLINK_GIT_DIR}/message_definitions/v1.0/${MAVLINK_DIALECT_UAVIONIX}.xml > ${CMAKE_CURRENT_BINARY_DIR}/mavgen_${MAVLINK_DIALECT_UAVIONIX}.log
@@ -60,7 +60,7 @@ set_source_files_properties(${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT_UAVIONIX}/$
 add_custom_command(
 	OUTPUT ${MAVLINK_LIBRARY_DIR}/${CONFIG_MAVLINK_DIALECT}/${CONFIG_MAVLINK_DIALECT}.h
 	COMMAND
-		${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
+		${Python_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
 			--lang C --wire-protocol 2.0
 			--output ${MAVLINK_LIBRARY_DIR}
 			${MAVLINK_GIT_DIR}/message_definitions/v1.0/${CONFIG_MAVLINK_DIALECT}.xml > ${CMAKE_CURRENT_BINARY_DIR}/mavgen_${CONFIG_MAVLINK_DIALECT}.log
@@ -81,6 +81,7 @@ MissionBlock::is_mission_item_reached_or_completed()
 	case NAV_CMD_DO_CONTROL_VIDEO:
 	case NAV_CMD_DO_MOUNT_CONFIGURE:
 	case NAV_CMD_DO_MOUNT_CONTROL:
+	case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
 	case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
 	case NAV_CMD_DO_SET_ROI:
 	case NAV_CMD_DO_SET_ROI_LOCATION:
@@ -141,14 +142,40 @@ MissionBlock::is_mission_item_reached_or_completed()

 		break;

-	case NAV_CMD_DO_WINCH:
-	case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
-	case NAV_CMD_DO_GRIPPER:
-		if (now > _timestamp_command_timeout + (_command_timeout * 1_s)) {
-			return true;
+	case NAV_CMD_DO_WINCH: {
+			const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) *
+					1E-6f; // TODO: Add proper microseconds_to_seconds function
+
+			if (_payload_deploy_ack_successful) {
+				PX4_DEBUG("Winch Deploy Ack received! Resuming mission");
+				return true;
+
+			} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
+				PX4_DEBUG("Winch Deploy Timed out, resuming mission!");
+				return true;
+
+			}
+
+			// We are still waiting for the acknowledgement / execution of deploy
+			return false;
 		}

-		return false; // Still waiting
+	case NAV_CMD_DO_GRIPPER: {
+			const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f;
+
+			if (_payload_deploy_ack_successful) {
+				PX4_DEBUG("Gripper Deploy Ack received! Resuming mission");
+				return true;
+
+			} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
+				PX4_DEBUG("Gripper Deploy Timed out, resuming mission!");
+				return true;
+
+			}
+
+			// We are still waiting for the acknowledgement / execution of deploy
+			return false;
+		}

 	default:
 		/* do nothing, this is a 3D waypoint */
@@ -418,7 +445,7 @@ MissionBlock::is_mission_item_reached_or_completed()

 		/* check if the MAV was long enough inside the waypoint orbit */
 		if ((get_time_inside(_mission_item) < FLT_EPSILON) ||
-		    (now >= (hrt_abstime)(get_time_inside(_mission_item) * 1_s) + _time_wp_reached)) {
+		    (now - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
 			time_inside_reached = true;
 		}

@@ -522,37 +549,53 @@ MissionBlock::issue_command(const mission_item_s &item)
 		return;
 	}

-	// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
-	if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {
-		_navigator->acquire_gimbal_control();
-	}
+	if (item.nav_cmd == NAV_CMD_DO_WINCH ||
+	    item.nav_cmd == NAV_CMD_DO_GRIPPER) {
+		// Initiate Payload Deployment
+		vehicle_command_s vcmd = {};
+		vcmd.command = item.nav_cmd;
+		vcmd.param1 = item.params[0];
+		vcmd.param2 = item.params[1];
+		vcmd.param3 = item.params[2];
+		vcmd.param4 = item.params[3];
+		vcmd.param5 = static_cast<double>(item.params[4]);
+		vcmd.param6 = static_cast<double>(item.params[5]);
+		_navigator->publish_vehicle_cmd(&vcmd);

-	// Mission item's NAV_CMD enums directly map to the according vehicle command
-	// So set the raw value directly (MAV_FRAME_MISSION mission item)
-	vehicle_command_s vcmd = {};
-	vcmd.command = item.nav_cmd;
-	vcmd.param1 = item.params[0];
-	vcmd.param2 = item.params[1];
-	vcmd.param3 = item.params[2];
-	vcmd.param4 = item.params[3];
-	vcmd.param5 = static_cast<double>(item.params[4]);
-	vcmd.param6 = static_cast<double>(item.params[5]);
-	vcmd.param7 = item.params[6];
+		// Reset payload deploy flag & data to get ready to receive deployment ack result
+		_payload_deploy_ack_successful = false;
+		_payload_deployed_time = hrt_absolute_time();

-	if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) {
-		// We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision
-		vcmd.param5 = item.lat;
-		vcmd.param6 = item.lon;
+	} else {

-		if (item.altitude_is_relative) {
-			vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
+		// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
+		if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {
+			_navigator->acquire_gimbal_control();
 		}
-	}

-	_navigator->publish_vehicle_cmd(&vcmd);
+		// Mission item's NAV_CMD enums directly map to the according vehicle command
+		// So set the raw value directly (MAV_FRAME_MISSION mission item)
+		vehicle_command_s vcmd = {};
+		vcmd.command = item.nav_cmd;
+		vcmd.param1 = item.params[0];
+		vcmd.param2 = item.params[1];
+		vcmd.param3 = item.params[2];
+		vcmd.param4 = item.params[3];
+		vcmd.param5 = static_cast<double>(item.params[4]);
+		vcmd.param6 = static_cast<double>(item.params[5]);
+		vcmd.param7 = item.params[6];

-	if (item_has_timeout(item)) {
-		_timestamp_command_timeout = hrt_absolute_time();
+		if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) {
+			// We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision
+			vcmd.param5 = item.lat;
+			vcmd.param6 = item.lon;
+
+			if (item.altitude_is_relative) {
+				vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
+			}
+		}
+
+		_navigator->publish_vehicle_cmd(&vcmd);
 	}
 }

@@ -576,13 +619,10 @@ MissionBlock::get_time_inside(const mission_item_s &item) const
 // and shouldn't have a timeout defined as it is a DO_* command. It should rather be defined as CONDITION_GRIPPER
 // or so, and have a function named 'item_is_conditional'
 // Reference: https://mavlink.io/en/services/mission.html#mavlink_commands
-// A similar condition applies to DO_GIMBAL_MANAGER_PITCHYAW
 bool
 MissionBlock::item_has_timeout(const mission_item_s &item)
 {
-	return item.nav_cmd == NAV_CMD_DO_WINCH ||
-	       item.nav_cmd == NAV_CMD_DO_GRIPPER ||
-	       item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
+	return item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER;
 }

 bool
--- a/Show More
+++ b/Show More