Use Rangefinder

A user configurable delay for the internal `vehicle_local_position` seems confusing in my eyes. It's a different timeout for fixed-wing and multirotor which might have made sense earlier but not really anymore since the topic is constantly published by the estimator and not expected to time out on either vehicle type and the parameter description is also misleading because it's outdated.

.github/workflows/dev_container.yml

@@ -25,8 +25,14 @@ 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 }}
@@ -44,16 +50,15 @@ jobs:
         with:
           images: |
             ghcr.io/PX4/px4-dev
-            px4io/px4-dev
+            ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }}
           tags: |
-            type=schedule
             type=semver,pattern={{version}}
             type=semver,pattern={{major}}.{{minor}}
             type=semver,pattern={{major}}
-            type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}},priority=600
+            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=,priority=500
             type=ref,event=pr
-            type=sha
 
       - name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v3

CMakeLists.txt

@@ -173,10 +173,9 @@ set(config_module_list)
 set(config_kernel_list)
 
 # Find Python
-find_package(Python COMPONENTS Interpreter)
-
+find_package(PythonInterp 3)
 # We have a custom error message to tell users how to install python3.
-if(NOT Python_FOUND)
+if(NOT PYTHONINTERP_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")
@@ -185,7 +184,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)
@@ -482,7 +481,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
@@ -503,7 +502,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
 )

Makefile

@@ -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

README.md

@@ -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)
 
-[![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)
+[![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)
 
 [![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode)
 

ROMFS/CMakeLists.txt

@@ -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

ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx

@@ -38,4 +38,6 @@ 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

ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane

@@ -45,7 +45,9 @@ 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_FUNC3 201
-param set-default PWM_MAIN_FUNC4 202
-param set-default PWM_MAIN_FUNC5 203
-param set-default PWM_MAIN_FUNC6 101
+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

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -11,6 +11,7 @@
 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

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -26,7 +26,6 @@ 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

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -29,7 +29,6 @@ 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
 

ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum

@@ -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_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_YAW_RATE_I 0.1
+param set-default RM_YAW_RATE_P 0.1
 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 4
-param set-default RM_MAX_THR_SPD 7
-param set-default RM_MAX_THR_YAW_R 7.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_YAW_P 5
 param set-default RM_YAW_I 0.1
-param set-default RM_MAX_YAW_RATE 180
-param set-default RM_MISS_SPD_DEF 3
+param set-default RM_MAX_YAW_RATE 120
+param set-default RM_MAX_YAW_ACCEL 240
 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 0
-param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_MIN1 70
+param set-default SIM_GZ_WH_MAX1 130
 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 0
-param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_MIN2 70
+param set-default SIM_GZ_WH_MAX2 130
 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 0
-param set-default SIM_GZ_WH_MAX3 200
+param set-default SIM_GZ_WH_MIN3 70
+param set-default SIM_GZ_WH_MAX3 130
 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 0
-param set-default SIM_GZ_WH_MAX4 200
+param set-default SIM_GZ_WH_MIN4 70
+param set-default SIM_GZ_WH_MAX4 130
 param set-default SIM_GZ_WH_DIS4 100
 
 param set-default SIM_GZ_WH_REV 10

ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter

@@ -0,0 +1,97 @@
+#!/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

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -89,6 +89,7 @@ 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

ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil

@@ -18,22 +18,19 @@ 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 4
-param set-default CA_SV_CS0_TRQ_R 0.5
-param set-default CA_SV_CS0_TYPE 2
+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_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-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
+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
 
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set-default SYS_HITL 2

ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover

@@ -31,7 +31,6 @@ 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

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -13,8 +13,6 @@ 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

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -211,6 +211,13 @@ 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

Tools/astyle/check_code_style_all.sh

@@ -43,7 +43,7 @@ fi
 
 # install git pre-commit hook
 HOOK_FILE="$DIR/../../.git/hooks/pre-commit"
-if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then
+if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ] && [ $- == *i* ]; 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]"

Tools/px4airframes/markdownout.py

@@ -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 = '<p><b>Specific Outputs:</b>' + outputs + '</p>'
+                        outputs_entry = '<br><b>Specific Outputs:</b>' + outputs
                     else:
                         outputs_entry = ''
 

boards/emlid/navio2/arm64.px4board

@@ -0,0 +1 @@
+CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"

boards/holybro/kakuteh7/init/rc.board_sensors

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

boards/holybro/kakuteh7mini/init/rc.board_sensors

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

boards/holybro/kakuteh7v2/init/rc.board_sensors

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

boards/mro/x21-777/default.px4board

@@ -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=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y

boards/nxp/tropic-community/cmake/upload.cmake

@@ -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

boards/px4/fmu-v6u/default.px4board

@@ -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=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y

cmake/kconfig.cmake

@@ -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}

cmake/metadata.cmake

@@ -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

cmake/px4_metadata.cmake

@@ -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

msg/CMakeLists.txt

@@ -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/

msg/RoverMecanumSetpoint.msg

@@ -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 yaw_rate_setpoint_normalized      # [-1, 1] Desired normalized yaw rate
+float32 speed_diff_setpoint_normalized    # [-1, 1] Normalized speed difference between the left and right wheels
 float32 yaw_setpoint 	                  # [rad] Desired yaw (heading)
 
 # TOPICS rover_mecanum_setpoint

msg/RoverMecanumStatus.msg

@@ -1,13 +1,17 @@
 uint64 timestamp # time since system start (microseconds)
 
-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
+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
 
 # TOPICS rover_mecanum_status

platforms/nuttx/CMakeLists.txt

@@ -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

platforms/nuttx/NuttX/tools/kconfig-conf

@@ -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

platforms/nuttx/cmake/upload.cmake

@@ -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

src/drivers/barometer/Kconfig

@@ -11,6 +11,7 @@ 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---

src/drivers/barometer/goertek/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   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
@@ -32,3 +32,4 @@
 ############################################################################
 
 add_subdirectory(spl06)
+add_subdirectory(spa06)

src/drivers/barometer/goertek/spa06/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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
+	)

src/drivers/barometer/goertek/spa06/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_BAROMETER_GOERTEK_SPA06
+	bool "spa06"
+	default n
+	---help---
+		Enable support for spa06

src/drivers/barometer/goertek/spa06/SPA06.cpp

@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ *   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);
+}

src/drivers/barometer/goertek/spa06/SPA06.hpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   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};
+};

src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   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

src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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

src/drivers/barometer/goertek/spa06/parameters.c

@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   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);

src/drivers/barometer/goertek/spa06/spa06.h

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   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

src/drivers/barometer/goertek/spa06/spa06_main.cpp

@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   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;
+}

src/drivers/bootloaders/CMakeLists.txt

@@ -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

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -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>
-#include <mathlib/mathlib.h>
-#include <matrix/matrix/math.hpp>
+
+using namespace time_literals;
 
 /* Configuration Constants */
-#define SF45_MAX_PAYLOAD 256
 #define SF45_SCALE_FACTOR 0.01f
+using namespace matrix;
 
-SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
+SF45LaserSerial::SF45LaserSerial(const char *port) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_px4_rangefinder(0, rotation),
+	_px4_rangefinder(0, distance_sensor_s::ROTATION_CUSTOM),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
 {
@@ -68,16 +68,9 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
 		device_id.devid_s.bus = bus_num;
 	}
 
-	_num_retries = 2;
 	_px4_rangefinder.set_device_id(device_id.devid);
 	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
-
-	// populate obstacle map members
-	_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
-	_obstacle_map_msg.increment = 5;
-	_obstacle_map_msg.angle_offset = 2.5;
-	_obstacle_map_msg.min_distance = UINT16_MAX;
-	_obstacle_map_msg.max_distance = 5000;
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
 
 }
 
@@ -97,47 +90,41 @@ 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()
 {
-
-	int rate = (int)_update_rate;
-	_data_output = 0x101; // raw distance + yaw readings
+	int32_t rate = (int32_t)_update_rate;
+	_data_output = 0x101; // raw distance (first return) + yaw readings
 	_stream_data = 5; // enable constant streaming
 
-	// send some packets so the sensor starts scanning
+	// send packets to the sensor depending on the state
 	switch (_sensor_state) {
 
-	// sensor should now respond
 	case STATE_UNINIT:
-		while (_num_retries--) {
-			sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
-			_sensor_state = STATE_UNINIT;
-		}
-
-		_sensor_state = STATE_SEND_PRODUCT_NAME;
+		// Used to probe if the sensor is alive
+		sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
 		break;
 
-	case STATE_SEND_PRODUCT_NAME:
+	case STATE_ACK_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_SEND_UPDATE_RATE:
+	case STATE_ACK_UPDATE_RATE:
+		// Configure the data that the sensor shall output
 		sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output));
-		_sensor_state = STATE_SEND_DISTANCE_DATA;
 		break;
 
-	case STATE_SEND_DISTANCE_DATA:
+	case STATE_ACK_DISTANCE_OUTPUT:
+		// Configure the sensor to automatically output data at the configured update rate
 		sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data));
 		_sensor_state = STATE_SEND_STREAM;
 		break;
@@ -151,129 +138,90 @@ 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;
 
-	/* read from the sensor (uart buffer) */
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	if (_sensor_state == STATE_UNINIT) {
 
+		perf_begin(_sample_perf);
+		const int payload_length = 22;
 
+		_crc_valid = false;
+		sf45_get_and_handle_request(payload_length, SF_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);
+		if (_crc_valid) {
+			_sensor_state = STATE_ACK_PRODUCT_NAME;
 			perf_end(_sample_perf);
-			return ret;
+			return PX4_OK;
 		}
 
-		sf45_request_handle(ret, readbuf);
-		ScheduleDelayed(_interval * 3);
+		return -EAGAIN;
 
-	} else if (_sensor_state == STATE_SEND_UPDATE_RATE) {
+	} else if (_sensor_state == STATE_ACK_PRODUCT_NAME) {
 
-		ret = ::read(_fd, &readbuf[0], 7);
+		perf_begin(_sample_perf);
+		const int payload_length = 7;
 
-		if (ret < 0) {
-			PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret);
-			perf_count(_comms_errors);
+		_crc_valid = false;
+		sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE);
+
+		if (_crc_valid) {
+			_sensor_state = STATE_ACK_UPDATE_RATE;
 			perf_end(_sample_perf);
-			return ret;
+			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) {
+	} else if (_sensor_state == STATE_ACK_UPDATE_RATE) {
 
-		ret = ::read(_fd, &readbuf[0], 8);
+		perf_begin(_sample_perf);
+		const int payload_length = 10;
 
-		if (ret < 0) {
-			PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret);
-			perf_count(_comms_errors);
+		_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 ret;
+			return PX4_OK;
 		}
 
-		if (readbuf[3] == SF_DISTANCE_OUTPUT) {
-			sf45_request_handle(ret, readbuf);
-			ScheduleDelayed(_interval * 3);
-		}
-
-		// Stream data from sensor
+		return -EAGAIN;
 
 	} else {
 
-		ret = ::read(_fd, &readbuf[0], 10);
+		// Stream data from sensor
+		perf_begin(_sample_perf);
+		const int payload_length = 10;
 
-		if (ret < 0) {
-			PX4_ERR("ERROR (ack from streaming distance data): %d", ret);
-			perf_count(_comms_errors);
+		_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"));
 			perf_end(_sample_perf);
-			return ret;
+			return PX4_OK;
 		}
 
-		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 report ring and state machine */
+	/* reset the sensor state */
+	_sensor_state = STATE_UNINIT;
+
+	/* reset the report ring */
 	_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();
 }
@@ -288,8 +236,7 @@ void SF45LaserSerial::Run()
 	/* fds initialized? */
 	if (_fd < 0) {
 		/* open fd: non-blocking read mode*/
-
-		_fd = ::open(_port, O_RDWR | O_NOCTTY);
+		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
 		if (_fd < 0) {
 			PX4_ERR("serial open failed (%i)", errno);
@@ -303,34 +250,11 @@ void SF45LaserSerial::Run()
 		/* fill the struct for the new configuration */
 		tcgetattr(_fd, &uart_config);
 
-		uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8;
+		/* clear ONLCR flag (which appends a CR for every LF) */
+		uart_config.c_oflag &= ~ONLCR;
 
-		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;
+		/* no parity, one stop bit */
+		uart_config.c_cflag &= ~(CSTOPB | PARENB);
 
 		unsigned speed = B921600;
 
@@ -346,51 +270,37 @@ void SF45LaserSerial::Run()
 		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
 			PX4_ERR("baud %d ATTR", termios_state);
 		}
-
 	}
 
 	if (_collect_phase) {
-
-		/* perform collection */
-		int collect_ret = collect();
-
-
-		if (collect_ret == -EAGAIN) {
-			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
-			ScheduleDelayed(1042 * 8);
-
-			return;
-		}
-
-		if (OK != collect_ret) {
-			//  Too many packet errors in init, restart the consecutive fail count
-			_consecutive_fail_count = 0;
-
-			/* restart the measurement state machine */
+		if (hrt_absolute_time() - _last_received_time >= 1_s) {
 			start();
 			return;
-
-		} else {
-			/* apparently success */
-			_consecutive_fail_count = 0;
 		}
 
-		/* next phase is measurement */
-		_collect_phase = false;
+		/* perform collection */
+		if (collect() != PX4_OK && errno != EAGAIN) {
+			PX4_DEBUG("collect error");
+		}
+
+		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;
 	}
 
-	/* 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()
@@ -399,9 +309,8 @@ void SF45LaserSerial::print_info()
 	perf_print_counter(_comms_errors);
 }
 
-void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
+void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id)
 {
-
 	// SF45 protocol
 	// Start byte is 0xAA and is the start of packet
 	// Payload length sanity check (0-1023) bytes
@@ -410,172 +319,178 @@ void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
 	// ID byte precedes the data in the payload
 	// CRC comprised of 16-bit checksum (not included in checksum calc.)
 
-	uint16_t recv_crc = 0;
-	bool restart_flag = false;
+	int ret;
+	size_t max_read = sizeof(_linebuf) - _linebuf_size;
+	ret = ::read(_fd, &_linebuf[_linebuf_size], max_read);
 
-	while (restart_flag != true) {
+	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;
+	}
 
-		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;
+	if (ret > 0) {
+		_last_received_time = hrt_absolute_time();
+		_linebuf_size += ret;
+	}
 
-				} 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
+	// Not enough data to parse a complete packet. Gather more data in the next cycle.
+	if (_linebuf_size < payload_length) {
+		return;
+	}
 
-		case 1: {
-				rx_field.flags_lo = input_buf[1];
-				_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
-				_parsed_state = 2;
-				break;
-			}
+	int index = 0;
 
-		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 (index <= _linebuf_size - payload_length && _crc_valid == false) {
+		bool restart_flag = false;
 
-				// 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;
+		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;
 
 					} else {
-						_sensor_ready = false;
+						_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
+
+			case 1: {
+					rx_field.flags_lo = _linebuf[index + 1];
+					_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
+					_parsed_state = 2;
+					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);
+
+					// 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];
+
+					if (rx_field.msg_id == msg_id) {
+						_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
+						_parsed_state = 4;
+						break;
 					}
 
-					_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
-
-					_parsed_state = 4;
-					break;
+					// 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;
+					}
 				}
 
-				// 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;
+			// 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;
 
-		// 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) {
+						}  // end for
+					} //end if
 
-						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;
+					else {
 
-					}  // end for
-				} //end if
-
-				else {
+						_parsed_state = 5;
+						_data_bytes_recv = 0;
+						_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
+					}
 
 					_parsed_state = 5;
 					_data_bytes_recv = 0;
-					_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
-
+					break;
 				}
 
-				_parsed_state = 5;
-				_data_bytes_recv = 0;
-				break;
-			}
+			// CRC low byte
+			case 5: {
+					rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len];
+					_parsed_state = 6;
+					break;
+				}
 
-		// CRC low byte
-		case 5: {
-				rx_field.crc[0] = input_buf[3 + rx_field.data_len];
-				_parsed_state = 6;
-				break;
-			}
+			// CRC high byte
+			case 6: {
+					rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len];
+					uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
 
-		// 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;
+					// 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;
 
 					} else {
-						_init_complete = false;
+
+						_crc_valid = false;
+						_parsed_state = 0;
+						_calc_crc = 0;
+						restart_flag = true;
+						perf_count(_comms_errors);
+						PX4_DEBUG("CRC mismatch: %d", _sensor_state);
+						break;
 					}
-
-					_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
+			} // 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);
+	}
 }
 
-void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len)
+void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8_t data_len)
 {
 	uint16_t crc_val = 0;
 	uint8_t packet_buff[SF45_MAX_PAYLOAD];
@@ -612,7 +527,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
 	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;
@@ -650,12 +565,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
 		// Add data bytes to crc add function
 		crc_val = sf45_format_crc(crc_val, data[0]);
 		data_inc = data_inc + 1;
-
 	}
 
 	else {
 		// Product Name
-		PX4_INFO("INFO: Product name");
+		PX4_DEBUG("DEBUG: Product name");
 	}
 
 	uint8_t crc_lo = crc_val & 0xFF;
@@ -683,12 +597,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
 	}
 }
 
-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;
@@ -696,11 +609,10 @@ 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 == 1) {
+			if (_orient_cfg == ROTATION_DOWNWARD_FACING) {
 				raw_yaw = raw_yaw * -1;
 			}
 
@@ -708,10 +620,10 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
 
 			switch (_yaw_cfg) {
-			case 0:
+			case ROTATION_FORWARD_FACING:
 				break;
 
-			case 1:
+			case ROTATION_BACKWARD_FACING:
 				if (scaled_yaw > 180) {
 					scaled_yaw = scaled_yaw - 180;
 
@@ -721,11 +633,11 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 
 				break;
 
-			case 2:
+			case ROTATION_RIGHT_FACING:
 				scaled_yaw = scaled_yaw + 90; // rotation facing right
 				break;
 
-			case 3:
+			case ROTATION_LEFT_FACING:
 				scaled_yaw = scaled_yaw - 90; // rotation facing left
 				break;
 
@@ -733,27 +645,21 @@ 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;
 
-			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
+			if (raw_distance < 65436) { // Discard invalid readings
+				// Convert to meters for rangefinder update
+				distance_m = raw_distance * SF45_SCALE_FACTOR;
 
-			// If we have moved to a new bin
+				Quatf quaternion(Eulerf{0, 0, sf45_wrap_360(scaled_yaw)*M_DEG_TO_RAD_F});
+				float q[4];
 
-			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();
+				for (int i = 0; i < 4; i++) {
+					q[i] = quaternion(i);
+				}
 
+				_px4_rangefinder.update(hrt_absolute_time(), distance_m, -1, q);
 			}
 
-			_previous_bin = current_bin;
-
-			_obstacle_distance_pub.publish(_obstacle_map_msg);
-
 			break;
 		}
 
@@ -763,16 +669,6 @@ 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);

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -56,33 +56,37 @@
 
 enum SF_SERIAL_STATE {
 	STATE_UNINIT = 0,
-	STATE_SEND_PRODUCT_NAME = 1,
-	STATE_SEND_UPDATE_RATE = 2,
-	STATE_SEND_DISTANCE_DATA = 3,
+	STATE_ACK_PRODUCT_NAME = 1,
+	STATE_ACK_UPDATE_RATE = 2,
+	STATE_ACK_DISTANCE_OUTPUT = 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, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	SF45LaserSerial(const char *port);
 	~SF45LaserSerial() override;
 
-	int 			        init();
+	int				init();
 	void				print_info();
-	void                            sf45_request_handle(int val, uint8_t *value);
-	void                            sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len);
-	uint16_t                        sf45_format_crc(uint16_t crc, uint8_t data_value);
-	void                            sf45_process_replies(float *data);
-	uint8_t                         sf45_convert_angle(const int16_t yaw);
-	float                           sf45_wrap_360(float f);
-protected:
-	obstacle_distance_s                       _obstacle_map_msg{};
-	uORB::Publication<obstacle_distance_s>		_obstacle_distance_pub{ORB_ID(obstacle_distance)};	/**< obstacle_distance publication */
+	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);
 
 private:
-
 	void				start();
 	void				stop();
 	void				Run() override;
@@ -91,41 +95,34 @@ private:
 	bool                            _crc_valid{false};
 	PX4Rangefinder                  _px4_rangefinder;
 
+
 	char 				_port[20] {};
-	int	        _interval{10000};
+	int				_interval{2000};
 	bool				_collect_phase{false};
 	int 				_fd{-1};
-	int         _linebuf[256] {};
-	unsigned		_linebuf_index{0};
-	hrt_abstime _last_read{0};
+	uint8_t				_linebuf[SF45_MAX_PAYLOAD] {};
+	int				_linebuf_size{0};
 
 	// SF45/B uses a binary protocol to include header,flags
 	// message ID, payload, and checksum
-	bool                            _is_sf45{false};
-	bool                            _init_complete{false};
-	bool                            _sensor_ready{false};
-	uint8_t                         _sensor_state{0};
-	int                             _baud_rate{0};
-	int                             _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-	int                             _stream_data{0};
-	int32_t                         _update_rate{1};
-	int                             _data_output{0};
-	const uint8_t                   _start_of_frame{0xAA};
-	uint16_t                        _data_bytes_recv{0};
-	uint8_t                         _parsed_state{0};
-	bool                            _sop_valid{false};
-	uint16_t                        _calc_crc{0};
-	uint8_t                         _num_retries{0};
-	int32_t                         _yaw_cfg{0};
-	int32_t                         _orient_cfg{0};
-	int32_t                         _collision_constraint{0};
-	uint16_t                        _previous_bin{0};
+	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};
 
 	// end of SF45/B data members
 
-	unsigned			                  _consecutive_fail_count;
-
-	perf_counter_t			            _sample_perf;
-	perf_counter_t			            _comms_errors;
-
+	hrt_abstime			_last_received_time{0};
+	perf_counter_t			_sample_perf;
+	perf_counter_t			_comms_errors;
 };

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp

@@ -41,7 +41,7 @@ namespace lightware_sf45
 
 SF45LaserSerial *g_dev{nullptr};
 
-static int start(const char *port, uint8_t rotation)
+static int start(const char *port)
 {
 	if (g_dev != nullptr) {
 		PX4_WARN("already started");
@@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation)
 	}
 
 	/* create the driver */
-	g_dev = new SF45LaserSerial(port, rotation);
+	g_dev = new SF45LaserSerial(port);
 
 	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/master/en/sensor/sfxx_lidar.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html
 
 ### Examples
 
@@ -116,7 +116,6 @@ $ 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;
 }
@@ -125,18 +124,13 @@ $ 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, "R:d:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		case 'R':
-			rotation = (uint8_t)atoi(myoptarg);
-			break;
-
 		case 'd':
 			device_path = myoptarg;
 			break;
@@ -153,7 +147,7 @@ extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[myoptind], "start")) {
-		return lightware_sf45::start(device_path, rotation);
+		return lightware_sf45::start(device_path);
 
 	} else if (!strcmp(argv[myoptind], "stop")) {
 		return lightware_sf45::stop();

src/drivers/distance_sensor/lightware_sf45_serial/module.yaml

@@ -32,7 +32,7 @@ parameters:
                   12: 5000hz
               reboot_required: true
               num_instances: 1
-              default: 1
+              default: 5
 
           SF45_ORIENT_CFG:
               description:
@@ -41,11 +41,11 @@ parameters:
                       The SF45 mounted facing upward or downward on the frame
               type: enum
               values:
-                0: Rotation upward
-                1: Rotation downward
+                24: Rotation upward
+                25: Rotation downward
               reboot_required: true
               num_instances: 1
-              default: 0
+              default: 24
 
           SF45_YAW_CFG:
               description:
@@ -55,9 +55,9 @@ parameters:
               type: enum
               values:
                 0: Rotation forward
-                1: Rotation backward
                 2: Rotation right
-                3: Rotation left
+                4: Rotation backward
+                6: Rotation left
               reboot_required: true
               num_instances: 1
               default: 0

src/drivers/drv_sensor.h

@@ -247,6 +247,7 @@
 
 #define DRV_DIFF_PRESS_DEVTYPE_AUAV	0xE6
 #define DRV_BARO_DEVTYPE_AUAV		0xE7
+#define DRV_BARO_DEVTYPE_SPA06          0xE8
 
 #define DRV_DEVTYPE_UNUSED		0xff
 

src/drivers/uavcan/CMakeLists.txt

@@ -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

src/drivers/uavcan/libdronecan/libuavcan/CMakeLists.txt

@@ -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}

src/drivers/uavcannode/CMakeLists.txt

@@ -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

src/lib/CMakeLists.txt

@@ -54,6 +54,7 @@ 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)

src/lib/component_information/CMakeLists.txt

@@ -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}

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -83,3 +83,32 @@ 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();
+}

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -63,6 +63,7 @@ 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(); };
 

src/lib/events/CMakeLists.txt

@@ -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}

src/lib/lat_lon_alt/lat_lon_alt.cpp

@@ -34,8 +34,52 @@
 #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;

src/lib/lat_lon_alt/lat_lon_alt.hpp

@@ -54,6 +54,10 @@ 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()); }
@@ -94,6 +98,14 @@ 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);
@@ -101,12 +113,6 @@ 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};

src/lib/lat_lon_alt/test_lat_lon_alt.cpp

@@ -105,3 +105,17 @@ 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;
+			}
+		}
+	}
+}

src/lib/matrix/matrix/Matrix.hpp

@@ -55,6 +55,16 @@ 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)
 	{

src/lib/metadata/CMakeLists.txt

@@ -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

src/lib/mixer_module/CMakeLists.txt

@@ -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

src/lib/motion_planning/PositionSmoothing.cpp

@@ -106,16 +106,32 @@ 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 auto &target = waypoints[1];
+	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)};
 
-	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
-			  _trajectory[1].getCurrentPosition(),
-			  _trajectory[2].getCurrentPosition());
+	float arrival_z_speed = 0.0f;
+	const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;
 
-	const float distance_start_target = fabs(target(2) - pos_traj(2));
-	const float arrival_z_speed = 0.f;
+	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 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));

src/lib/motion_planning/PositionSmoothingTest.cpp

@@ -151,14 +151,17 @@ TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration)
 
 TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
 {
-	const int N_ITER = 2000;
+	const int N_ITER = 20000;
 	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, NEXT_TARGET};
+
+	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET};
 	Vector3f ff_velocity{1.f, 0.1f, 0.3f};
 
 	Vector3f position{0.f, 0.f, 0.f};
@@ -180,12 +183,13 @@ TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
 		ff_velocity = {0.f, 0.f, 0.f};
 		expectDynamicsLimitsRespected(out);
 
-		if (position == TARGET) {
+		if (Vector2f(position.xy() - TARGET.xy()).norm() < XY_ACC_RAD && fabsf(position(2) - TARGET(2)) < Z_ACC_RAD) {
 			printf("Converged in %d iterations\n", iteration);
 			break;
 		}
 	}
 
-	EXPECT_EQ(TARGET, position);
+	EXPECT_LT(Vector2f(position.xy() - TARGET.xy()).norm(), XY_ACC_RAD);
+	EXPECT_LT(fabsf(position(2) - TARGET(2)), Z_ACC_RAD);
 	EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n";
 }

src/lib/motion_planning/TrajectoryConstraints.hpp

@@ -74,15 +74,11 @@ 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 &&
-	    has_reached_altitude &&
-	    altitude_stays_same
+	    !waypoint_overlap
 	   ) {
 		const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot(
 						  Vector2f((target - next_target).xy()).unit_or_zero()));
@@ -108,15 +104,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 <size_t N>
+template <int 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;
 
-	for (size_t j = 0; j < N - 1; j++) {
-		size_t i = N - 2 - j;
+	// go backwards through the waypoints
+	for (int i = (N - 2); i >= 0; i--) {
 		max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
 				waypoints[i + 1],
 				waypoints[min(i + 2, N - 1)],

src/lib/parameters/CMakeLists.txt

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

src/lib/pid/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-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
@@ -31,4 +31,10 @@
 #
 ############################################################################
 
-px4_add_library(pid pid.cpp)
+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)

src/lib/pid/PID.cpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   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;
+}

src/lib/pid/PID.hpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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};
+};

src/lib/pid/PIDTest.cpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   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);
+}

src/lib/pid/pid.cpp

@@ -1,185 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}

src/lib/pid/pid.h

@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   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_ */

src/lib/version/CMakeLists.txt

@@ -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

src/lib/wind_estimator/CMakeLists.txt

@@ -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

src/lib/world_magnetic_model/CMakeLists.txt

@@ -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

src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp

@@ -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);

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -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 + (_param_com_pos_fs_delay.get() * 1_s));
+	failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + 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 + _param_com_pos_fs_delay.get() * 1_s) || (data_timestamp_us == 0);
+	const bool data_stale = (now > data_timestamp_us + 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);
 

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -118,7 +118,6 @@ 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
 				       )

src/modules/commander/commander_params.c

@@ -512,19 +512,6 @@ 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.
  *

src/modules/commander/failsafe/CMakeLists.txt

@@ -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

src/modules/ekf2/CMakeLists.txt

@@ -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

src/modules/ekf2/EKF/CMakeLists.txt

@@ -33,7 +33,6 @@
 
 add_subdirectory(bias_estimator)
 add_subdirectory(output_predictor)
-add_subdirectory(lat_lon_alt)
 
 set(EKF_LIBS)
 set(EKF_SRCS)

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -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, sample.eph,
-								      sample.epv)) {
+					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
+								      sq(sample.epv))) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;

src/modules/ekf2/EKF/ekf.h

@@ -66,8 +66,6 @@
 # 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;
 
@@ -193,9 +191,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 eph = NAN, float epv = NAN);
-	bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN,
-				   float epv = NAN);
+	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);
 
 	// 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;

src/modules/ekf2/EKF/estimator_interface.h

@@ -42,7 +42,6 @@
 #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
@@ -73,6 +72,7 @@
 #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>

src/modules/ekf2/EKF/output_predictor/output_predictor.h

@@ -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
 {

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -337,9 +337,7 @@ def predict_sideslip(
     vel_rel = state["vel"] - wind
     relative_wind_body = state["quat_nominal"].inverse() * vel_rel
 
-    # 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)
+    sideslip_pred = sf.atan2(relative_wind_body[1], relative_wind_body[0], epsilon)
 
     return sideslip_pred
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h

@@ -30,66 +30,65 @@ 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: 513
+  // Total ops: 518
 
   // Input arrays
 
-  // 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);
+  // 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);
   const Scalar _tmp30 =
-      -_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));
+      _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));
 
   // Output terms (2)
   if (H != nullptr) {
@@ -97,91 +96,91 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
 
     _h.setZero();
 
-    _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;
+    _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;
   }
 
   if (K != nullptr) {
     matrix::Matrix<Scalar, 24, 1>& _k = (*K);
 
     _k(0, 0) =
-        _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);
+        _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);
     _k(1, 0) =
-        _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);
+        _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);
     _k(2, 0) =
-        _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);
+        _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);
     _k(3, 0) =
-        _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);
+        _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);
     _k(4, 0) =
-        _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);
+        _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);
     _k(5, 0) =
-        _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);
+        _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);
     _k(6, 0) =
-        _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);
+        _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);
     _k(7, 0) =
-        _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);
+        _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);
     _k(8, 0) =
-        _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);
+        _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);
     _k(9, 0) =
-        _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);
+        _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);
     _k(10, 0) =
-        _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);
+        _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);
     _k(11, 0) =
-        _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);
+        _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);
     _k(12, 0) =
-        _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);
+        _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);
     _k(13, 0) =
-        _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);
+        _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);
     _k(14, 0) =
-        _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);
+        _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);
     _k(15, 0) =
-        _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);
+        _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);
     _k(16, 0) =
-        _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);
+        _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);
     _k(17, 0) =
-        _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);
+        _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);
     _k(18, 0) =
-        _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);
+        _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);
     _k(19, 0) =
-        _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);
+        _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);
     _k(20, 0) =
-        _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);
+        _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);
     _k(21, 0) =
-        _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);
+        _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);
     _k(22, 0) =
-        _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);
+        _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);
     _k(23, 0) =
-        _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);
+        _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);
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h

@@ -30,70 +30,69 @@ 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: 265
+  // Total ops: 266
 
   // Input arrays
 
-  // Intermediate terms (42)
+  // Intermediate terms (49)
   const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  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 _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 _tmp5 = 2 * state(1, 0);
   const Scalar _tmp6 = _tmp5 * state(2, 0);
-  const Scalar _tmp7 = _tmp4 + _tmp6;
-  const Scalar _tmp8 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp7 = -_tmp4 + _tmp6;
+  const Scalar _tmp8 = -state(22, 0) + state(4, 0);
   const Scalar _tmp9 = 2 * state(2, 0);
-  const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp10 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
   const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
-  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;
+  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);
 
   // Output terms (2)
   if (innov != nullptr) {
     Scalar& _innov = (*innov);
 
-    _innov = _tmp13 * _tmp17;
+    _innov = std::atan2(_tmp11, _tmp16);
   }
 
   if (innov_var != nullptr) {
@@ -101,22 +100,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
 
     _innov_var =
         R +
-        _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);
+        _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);
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/test/test_EKF_flow.cpp

@@ -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, epv);
+	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph * eph, epv * epv);
 
 	const Vector3f lpos_after_reset = _ekf->getPosition();
 

src/modules/flight_mode_manager/CMakeLists.txt

@@ -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

src/modules/logger/logged_topics.cpp

@@ -323,7 +323,9 @@ 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);

src/modules/mavlink/CMakeLists.txt

@@ -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

src/modules/navigator/mission_block.cpp

@@ -81,7 +81,6 @@ 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:
@@ -142,40 +141,14 @@ MissionBlock::is_mission_item_reached_or_completed()
 
 		break;
 
-	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;
+	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_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;
-		}
+		return false; // Still waiting
 
 	default:
 		/* do nothing, this is a 3D waypoint */
@@ -445,7 +418,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 - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
+		    (now >= (hrt_abstime)(get_time_inside(_mission_item) * 1_s) + _time_wp_reached)) {
 			time_inside_reached = true;
 		}
 
@@ -549,53 +522,37 @@ MissionBlock::issue_command(const mission_item_s &item)
 		return;
 	}
 
-	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);
+	// 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();
+	}
 
-		// Reset payload deploy flag & data to get ready to receive deployment ack result
-		_payload_deploy_ack_successful = false;
-		_payload_deployed_time = hrt_absolute_time();
+	// 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];
 
-	} else {
+	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;
 
-		// 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.altitude_is_relative) {
+			vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
 		}
+	}
 
-		// 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];
+	_navigator->publish_vehicle_cmd(&vcmd);
 
-		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);
+	if (item_has_timeout(item)) {
+		_timestamp_command_timeout = hrt_absolute_time();
 	}
 }
 
@@ -619,10 +576,13 @@ 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;
+	return item.nav_cmd == NAV_CMD_DO_WINCH ||
+	       item.nav_cmd == NAV_CMD_DO_GRIPPER ||
+	       item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
 }
 
 bool