cmake: move FindPythonInterp->FindPython

- cmake CMP0148 FindPythonInterp deprecated
2026-05-24 10:47:34 +08:00 · 2024-11-22 21:57:53 -05:00
82 changed files with 929 additions and 860 deletions
@@ -25,14 +25,8 @@ jobs:
          submodules: false
          fetch-depth: 0

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

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

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

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

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

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

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

 # Functions
@@ -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
@@ -45,7 +45,7 @@ param set-default CA_SV_CS1_TRQ_P 1
 param set-default CA_SV_CS1_TYPE 3
 param set-default CA_SV_CS2_TRQ_Y 1
 param set-default CA_SV_CS2_TYPE 4
-param set-default PWM_MAIN_FUNC1 201
-param set-default PWM_MAIN_FUNC2 202
-param set-default PWM_MAIN_FUNC3 203
-param set-default PWM_MAIN_FUNC4 101
+param set-default 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
@@ -43,7 +43,7 @@ fi

 # install git pre-commit hook
 HOOK_FILE="$DIR/../../.git/hooks/pre-commit"
-if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ] && [ $- == *i* ]; then
+if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then
 	echo ""
 	echo -e "\033[31mNinja tip: add a git pre-commit hook to automatically check code style\033[0m"
 	echo -e "Would you like to install one now? (\033[94mcp ./Tools/astyle/pre-commit .git/hooks/pre-commit\033[0m): [y/\033[1mN\033[0m]"
@@ -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
@@ -1,20 +1,20 @@
 set(BOARD_DEFCONFIG ${PX4_CONFIG_FILE} CACHE FILEPATH "path to defconfig" FORCE)
 set(BOARD_CONFIG ${PX4_BINARY_DIR}/boardconfig CACHE FILEPATH "path to config" FORCE)

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

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

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

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

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

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

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

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

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

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

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

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

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

 if(CONFIG_MODULES_ZENOH)
 	# Update kconfig file for topics
-	execute_process(COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
+	execute_process(COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
 			--zenoh-config
 			-f ${msg_files}
 			-o ${PX4_SOURCE_DIR}/src/modules/zenoh/
@@ -522,7 +522,7 @@ if(CONFIG_MODULES_ZENOH)
 	add_custom_command(
 		OUTPUT
 			${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp
-		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
+		COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
 			--zenoh-pub-sub
 			-f ${msg_files}
 			-o ${PX4_BINARY_DIR}/src/modules/zenoh/
@@ -328,7 +328,7 @@ if (TARGET parameters_xml AND TARGET airframes_xml)
 	add_custom_command(
 		OUTPUT ${fw_package}
 		COMMAND
-			${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_mkfw.py
+			${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_mkfw.py
 				--prototype ${PX4_SOURCE_DIR}/boards/${PX4_BOARD_VENDOR}/${PX4_BOARD_MODEL}/firmware.prototype
 				--git_identity ${PX4_SOURCE_DIR}
 				--parameter_xml ${PX4_BINARY_DIR}/parameters.xml
@@ -6,13 +6,13 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
 export APPSDIR="`pwd`/../apps"

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

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

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

 add_custom_target(force-upload
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --force --port ${serial_ports} ${fw_package}
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --force --port ${serial_ports} ${fw_package}
 	DEPENDS ${fw_package}
 	COMMENT "uploading px4 with --force"
 	VERBATIM
@@ -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
@@ -120,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
 endforeach(DSDLC_INPUT)
 add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp
 	COMMAND
-		${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
+		${Python_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
 			--outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS}
 			#--verbose
 	COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
@@ -42,7 +42,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
    set(DSDLC_INPUT_FILES ${DSDLC_INPUT_FILES} ${DSDLC_NEW_INPUT_FILES})
 endforeach(DSDLC_INPUT)
 add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp
-                   COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
+                   COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
                   COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp
                   DEPENDS ${DSDLC_INPUT_FILES}
                   WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
@@ -120,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
 endforeach(DSDLC_INPUT)
 add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp
 	COMMAND
-		${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
+		${Python_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
 			--outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS}
 			#--verbose
 	COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
@@ -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}
@@ -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}
@@ -39,14 +39,14 @@ endif()
 set(generated_actuators_metadata_file ${PX4_BINARY_DIR}/actuators.json)
 add_custom_command(OUTPUT ${generated_actuators_metadata_file}
 	${generated_actuators_metadata_file}.xz
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_actuators_metadata.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_actuators_metadata.py
 		${board_with_io_arg}
 		--timer-config ${PX4_BOARD_DIR}/src/timer_config.cpp
 		--config-files ${module_config_files} #--verbose
 		--compress
 		--board ${PX4_BOARD}
 		--output-file ${generated_actuators_metadata_file}
-	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
+	COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
 		--schema-file ${PX4_SOURCE_DIR}/src/modules/mavlink/mavlink/component_information/actuators.schema.json
 		${generated_actuators_metadata_file}
 		--skip-if-no-schema # mavlink submodule might not exist for current target if built in CI
@@ -33,7 +33,7 @@

 set(functions_header ${CMAKE_CURRENT_BINARY_DIR}/output_functions.hpp)
 add_custom_command(OUTPUT ${functions_header}
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_function_header.py
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_function_header.py
 		${CMAKE_CURRENT_SOURCE_DIR}/output_functions.yaml
 		${functions_header}
 	DEPENDS
@@ -90,12 +90,12 @@ endif()

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

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

 list(APPEND SRCS
-	parameters.cpp 
+	parameters.cpp
 	atomic_transaction.cpp
 	autosave.cpp
 )
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,10 +31,4 @@
 #
 ############################################################################

-px4_add_library(PID
-	PID.cpp
-	PID.hpp
-)
-target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)
+px4_add_library(pid pid.cpp)
@@ -1,75 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "PID.hpp"
-#include "lib/mathlib/math/Functions.hpp"
-
-void PID::setGains(const float P, const float I, const float D)
-{
-	_gain_proportional = P;
-	_gain_integral = I;
-	_gain_derivative = D;
-}
-
-float PID::update(const float feedback, const float dt, const bool update_integral)
-{
-	const float error = _setpoint - feedback;
-	const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt));
-
-	if (update_integral) {
-		updateIntegral(error, dt);
-	}
-
-	_last_feedback = feedback;
-	return math::constrain(output, -_limit_output, _limit_output);
-}
-
-void PID::updateIntegral(float error, const float dt)
-{
-	const float integral_new = _integral + _gain_integral * error * dt;
-
-	if (std::isfinite(integral_new)) {
-		_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
-	}
-}
-
-float PID::updateDerivative(float feedback, const float dt)
-{
-	float feedback_change = 0.f;
-
-	if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) {
-		feedback_change = (feedback - _last_feedback) / dt;
-	}
-
-	return feedback_change;
-}
@@ -1,65 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <cmath>
-
-class PID
-{
-public:
-	PID() = default;
-	virtual ~PID() = default;
-	void setOutputLimit(const float limit) { _limit_output = limit; }
-	void setIntegralLimit(const float limit) { _limit_integral = limit; }
-	void setGains(const float P, const float I, const float D);
-	void setSetpoint(const float setpoint) { _setpoint = setpoint; }
-	float update(const float feedback, const float dt, const bool update_integral = true);
-	float getIntegral() { return _integral; }
-	void resetIntegral() { _integral = 0.f; };
-	void resetDerivative() { _last_feedback = NAN; };
-private:
-	void updateIntegral(float error, const float dt);
-	float updateDerivative(float feedback, const float dt);
-
-	float _setpoint{0.f}; ///< current setpoint to track
-	float _integral{0.f}; ///< integral state
-	float _last_feedback{NAN};
-
-	// Gains, Limits
-	float _gain_proportional{0.f};
-	float _gain_integral{0.f};
-	float _gain_derivative{0.f};
-	float _limit_integral{0.f};
-	float _limit_output{0.f};
-};
@@ -1,130 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include <gtest/gtest.h>
-#include <PID.hpp>
-
-TEST(PIDTest, AllZeroCase)
-{
-	PID pid;
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
-}
-
-TEST(PIDTest, OutputLimit)
-{
-	PID pid;
-	pid.setOutputLimit(.01f);
-	pid.setGains(.1f, 0.f, 0.f);
-	pid.setSetpoint(1.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
-	EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
-	EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
-	EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
-	EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
-	EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
-	EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
-}
-
-TEST(PIDTest, ProportinalOnly)
-{
-	PID pid;
-	pid.setOutputLimit(1.f);
-	pid.setGains(.1f, 0.f, 0.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
-	pid.setSetpoint(1.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
-
-	float plant = 0.f;
-	float output = 10000.f;
-	int i; // need function scope to check how many steps
-
-	for (i = 1000; i > 0; i--) {
-		const float output_new = pid.update(plant, 0.f, false);
-		plant += output_new;
-
-		// expect the output to get smaller with each iteration
-		if (output_new >= output) {
-			break;
-		}
-
-		output = output_new;
-	}
-
-	EXPECT_FLOAT_EQ(plant, 1.f);
-	EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
-}
-
-TEST(PIDTest, InteralOpenLoop)
-{
-	PID pid;
-	pid.setOutputLimit(1.f);
-	pid.setGains(0.f, .1f, 0.f);
-	pid.setIntegralLimit(.05f);
-	pid.setSetpoint(1.f);
-
-	// Zero error
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
-	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
-
-	// Open loop ramp up
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
-
-	// Open loop ramp down
-	pid.setSetpoint(-1.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
-	EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
-	EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
-	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
-	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
-	pid.resetIntegral();
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
-	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
-}
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pid.cpp
+ *
+ * Implementation of generic PID controller.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include "pid.h"
+#include <math.h>
+#include <px4_platform_common/defines.h>
+
+#define SIGMA 0.000001f
+
+__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
+{
+	pid->mode = mode;
+	pid->dt_min = dt_min;
+	pid->kp = 0.0f;
+	pid->ki = 0.0f;
+	pid->kd = 0.0f;
+	pid->integral = 0.0f;
+	pid->integral_limit = 0.0f;
+	pid->output_limit = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->last_output = 0.0f;
+}
+
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
+{
+	int ret = 0;
+
+	if (PX4_ISFINITE(kp)) {
+		pid->kp = kp;
+
+	} else {
+		ret = 1;
+	}
+
+	if (PX4_ISFINITE(ki)) {
+		pid->ki = ki;
+
+	} else {
+		ret = 1;
+	}
+
+	if (PX4_ISFINITE(kd)) {
+		pid->kd = kd;
+
+	} else {
+		ret = 1;
+	}
+
+	if (PX4_ISFINITE(integral_limit)) {
+		pid->integral_limit = integral_limit;
+
+	}  else {
+		ret = 1;
+	}
+
+	if (PX4_ISFINITE(output_limit)) {
+		pid->output_limit = output_limit;
+
+	}  else {
+		ret = 1;
+	}
+
+	return ret;
+}
+
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
+{
+	if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
+		return pid->last_output;
+	}
+
+	float i, d;
+
+	/* current error value */
+	float error = sp - val;
+
+	/* current error derivative */
+	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
+		d = -val_dot;
+
+	} else {
+		d = 0.0f;
+	}
+
+	if (!PX4_ISFINITE(d)) {
+		d = 0.0f;
+	}
+
+	/* calculate PD output */
+	float output = (error * pid->kp) + (d * pid->kd);
+
+	if (pid->ki > SIGMA) {
+		// Calculate the error integral and check for saturation
+		i = pid->integral + (error * dt);
+
+		/* check for saturation */
+		if (PX4_ISFINITE(i)) {
+			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
+			    fabsf(i) <= pid->integral_limit) {
+				/* not saturated, use new integral value */
+				pid->integral = i;
+			}
+		}
+
+		/* add I component to output */
+		output += pid->integral * pid->ki;
+	}
+
+	/* limit output */
+	if (PX4_ISFINITE(output)) {
+		if (pid->output_limit > SIGMA) {
+			if (output > pid->output_limit) {
+				output = pid->output_limit;
+
+			} else if (output < -pid->output_limit) {
+				output = -pid->output_limit;
+			}
+		}
+
+		pid->last_output = output;
+	}
+
+	return pid->last_output;
+}
+
+
+__EXPORT void pid_reset_integral(PID_t *pid)
+{
+	pid->integral = 0.0f;
+}
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pid.h
+ *
+ * Definition of generic PID controller.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+typedef enum PID_MODE {
+	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
+	PID_MODE_DERIVATIV_NONE = 0,
+	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
+	 * val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC,
+	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
+	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC_NO_SP,
+	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
+	PID_MODE_DERIVATIV_SET
+} pid_mode_t;
+
+typedef struct {
+	pid_mode_t mode;
+	float dt_min;
+	float kp;
+	float ki;
+	float kd;
+	float integral;
+	float integral_limit;
+	float output_limit;
+	float error_previous;
+	float last_output;
+} PID_t;
+
+__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
+__EXPORT void pid_reset_integral(PID_t *pid);
+
+__END_DECLS
+
+#endif /* PID_H_ */
@@ -54,7 +54,7 @@ endif()
 set(px4_git_ver_header ${CMAKE_CURRENT_BINARY_DIR}/build_git_version.h)
 add_custom_command(OUTPUT ${px4_git_ver_header}
 	COMMAND
-		${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate --git_tag '${PX4_GIT_TAG}'
+		${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate --git_tag '${PX4_GIT_TAG}'
 	DEPENDS
 		${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py
 		${git_dir_path}/HEAD
@@ -40,7 +40,7 @@ add_dependencies(wind_estimator prebuild_targets)
 add_subdirectory(test)

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

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

 add_custom_target(world_magnetic_model_tests_update
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
 	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	USES_TERMINAL
@@ -37,7 +37,7 @@ set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_
 set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html)
 set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html)
 add_custom_command(OUTPUT ${generated_uorb_struct_field_mapping_header} ${html_output_file}
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
+	COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
 		${failsafe_flags_msg_file} ${generated_uorb_struct_field_mapping_header}
 		${html_template_file} ${html_output_file}
 	DEPENDS
@@ -35,7 +35,7 @@ option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF)

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

 #include <mathlib/mathlib.h>

+bool Ekf::init(uint64_t timestamp)
+{
+	if (!_initialised) {
+		_initialised = initialise_interface(timestamp);
+		reset();
+	}
+
+	return _initialised;
+}
+
 void Ekf::reset()
 {
 	ECL_INFO("reset");

-	_state = {};
 	_state.quat_nominal.setIdentity();
+	_state.vel.setZero();
+	_state.pos.setZero();
+	_state.gyro_bias.setZero();
+	_state.accel_bias.setZero();

+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	_state.mag_I.setZero();
+	_state.mag_B.setZero();
+#endif // CONFIG_EKF2_MAGNETOMETER
+
+#if defined(CONFIG_EKF2_WIND)
+	_state.wind_vel.setZero();
+#endif // CONFIG_EKF2_WIND
+	//
 #if defined(CONFIG_EKF2_TERRAIN)
 	// assume a ground clearance
 	_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
@@ -79,13 +101,6 @@ void Ekf::reset()

 	_output_predictor.reset();

-	_time_latest_us = 0;
-	_time_delayed_us = 0;
-
-	_imu_updated = false;
-
-	_filter_initialised = false;
-
 	// Ekf private fields
 	_time_last_horizontal_aiding = 0;
 	_time_last_v_pos_aiding = 0;
@@ -118,12 +133,18 @@ void Ekf::reset()
 	_zero_velocity_update.reset();

 	updateParameters();
-
-	initialiseCovariance();
 }

 bool Ekf::update()
 {
+	if (!_filter_initialised) {
+		_filter_initialised = initialiseFilter();
+
+		if (!_filter_initialised) {
+			return false;
+		}
+	}
+
 	// Only run the filter if IMU data in the buffer has been updated
 	if (_imu_updated) {
 		_imu_updated = false;
@@ -138,52 +159,14 @@ bool Ekf::update()
 		float filter_update_s = 1e-6f * _params.filter_update_interval_us;
 		_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);

-		// Filter accel for tilt initialization
-		if (_is_first_imu_sample) {
-			_accel_lpf.reset(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt);
-			_gyro_lpf.reset(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt);
-			_is_first_imu_sample = false;
-
-			_state = {};
-			initialiseTilt(_accel_lpf.getState());
-			initialiseCovariance();
-
-		} else {
-			_accel_lpf.update(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt);
-			_gyro_lpf.update(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt);
-		}
-
-		if (!_filter_initialised) {
-
-			const float accel_norm = _accel_lpf.getState().norm();
-			const float gyro_norm = _gyro_lpf.getState().norm();
-
-			if ((accel_norm > 0.8f * CONSTANTS_ONE_G)
-			    && (accel_norm < 1.2f * CONSTANTS_ONE_G)
-			    && (gyro_norm < math::radians(15.f))
-			   ) {
-				// once ready reset and init tilt from filtered accel
-				_state = {};
-				initialiseTilt(_accel_lpf.getState());
-				initialiseCovariance();
-
-				// reset the output predictor state history to match the EKF initial values
-				_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos);
-
-				_filter_initialised = true;
-			}
-		}
-
 		updateIMUBiasInhibit(imu_sample_delayed);

 		// perform state and covariance prediction for the main filter
 		predictCovariance(imu_sample_delayed);
 		predictState(imu_sample_delayed);

-		if (_filter_initialised) {
-			// control fusion of observation data
-			controlFusionModes(imu_sample_delayed);
-		}
+		// control fusion of observation data
+		controlFusionModes(imu_sample_delayed);

 		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos,
 						      _state.gyro_bias, _state.accel_bias);
@@ -194,10 +177,52 @@ bool Ekf::update()
 	return false;
 }

-bool Ekf::initialiseTilt(const Vector3f &accel)
+bool Ekf::initialiseFilter()
 {
+	// Filter accel for tilt initialization
+	const imuSample &imu_init = _imu_buffer.get_newest();
+
+	// protect against zero data
+	if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
+		return false;
+	}
+
+	if (_is_first_imu_sample) {
+		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
+		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
+		_is_first_imu_sample = false;
+
+	} else {
+		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
+		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
+	}
+
+	if (!initialiseTilt()) {
+		return false;
+	}
+
+	// initialise the state covariance matrix now we have starting values for all the states
+	initialiseCovariance();
+
+	// reset the output predictor state history to match the EKF initial values
+	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos);
+
+	return true;
+}
+
+bool Ekf::initialiseTilt()
+{
+	const float accel_norm = _accel_lpf.getState().norm();
+	const float gyro_norm = _gyro_lpf.getState().norm();
+
+	if (accel_norm < 0.8f * CONSTANTS_ONE_G ||
+	    accel_norm > 1.2f * CONSTANTS_ONE_G ||
+	    gyro_norm > math::radians(15.0f)) {
+		return false;
+	}
+
 	// get initial tilt estimate from delta velocity vector, assuming vehicle is static
-	_state.quat_nominal = Quatf(accel, Vector3f(0.f, 0.f, -1.f));
+	_state.quat_nominal = Quatf(_accel_lpf.getState(), Vector3f(0.f, 0.f, -1.f));
 	_R_to_earth = Dcmf(_state.quat_nominal);

 	return true;
@@ -80,15 +80,15 @@ public:
 	Ekf()
 	{
 		reset();
-		initialise_interface();
 	};

-	~Ekf() = default;
+	virtual ~Ekf() = default;
+
+	// initialise variables to sane values (also interface class)
+	bool init(uint64_t timestamp) override;

 	void print_status();

-	void reset();
-
 	// should be called every time new data is pushed into the filter
 	bool update();

@@ -427,7 +427,10 @@ private:
 	friend class EvVelLocalFrameNed;
 	friend class EvVelLocalFrameFrd;

-	bool initialiseTilt(const Vector3f &accel);
+	// set the internal states and status to their default value
+	void reset();
+
+	bool initialiseTilt();

 	// check if the EKF is dead reckoning horizontal velocity using inertial data only
 	void updateDeadReckoningStatus();
@@ -580,6 +583,11 @@ private:
 	estimator_aid_source2d_s _aid_src_aux_vel {};
 #endif // CONFIG_EKF2_AUXVEL

+	// Variables used by the initial filter alignment
+	bool _is_first_imu_sample{true};
+	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
+	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+
 #if defined(CONFIG_EKF2_BAROMETER)
 	estimator_aid_source1d_s _aid_src_baro_hgt {};

@@ -618,6 +626,9 @@ private:
 	uint64_t _time_good_vert_accel{0};	///< last time a good vertical accel was detected (uSec)
 	uint16_t _clip_counter[3];		///< counter per axis that increments when clipping ad decrements when not

+	// initialise filter states of both the delayed ekf and the real time complementary filter
+	bool initialiseFilter(void);
+
 	// initialise ekf covariance matrix
 	void initialiseCovariance();

@@ -78,6 +78,11 @@ EstimatorInterface::~EstimatorInterface()
 // Accumulate imu data and store to buffer at desired rate
 void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 {
+	// TODO: resolve misplaced responsibility
+	if (!_initialised) {
+		_initialised = init(imu_sample.time_us);
+	}
+
 	_time_latest_us = imu_sample.time_us;

 	// the output observer always runs
@@ -117,19 +122,18 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 void EstimatorInterface::setMagData(const magSample &mag_sample)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_mag_buffer == nullptr) {
-		if (_obs_buffer_length > 0) {
-			_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
+		_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);

-			if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
-				delete _mag_buffer;
-				_mag_buffer = nullptr;
-				printBufferAllocationFailed("mag");
-				return;
-			}
-
-		} else {
+		if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
+			delete _mag_buffer;
+			_mag_buffer = nullptr;
+			printBufferAllocationFailed("mag");
 			return;
 		}
 	}
@@ -157,19 +161,18 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 #if defined(CONFIG_EKF2_GNSS)
 void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_gps_buffer == nullptr) {
-		if (_obs_buffer_length > 0) {
-			_gps_buffer = new RingBuffer<gnssSample>(_obs_buffer_length);
+		_gps_buffer = new RingBuffer<gnssSample>(_obs_buffer_length);

-			if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
-				delete _gps_buffer;
-				_gps_buffer = nullptr;
-				printBufferAllocationFailed("GPS");
-				return;
-			}
-
-		} else {
+		if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
+			delete _gps_buffer;
+			_gps_buffer = nullptr;
+			printBufferAllocationFailed("GPS");
 			return;
 		}
 	}
@@ -205,19 +208,18 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 #if defined(CONFIG_EKF2_BAROMETER)
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_baro_buffer == nullptr) {
-		if (_obs_buffer_length > 0) {
-			_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
+		_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);

-			if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
-				delete _baro_buffer;
-				_baro_buffer = nullptr;
-				printBufferAllocationFailed("baro");
-				return;
-			}
-
-		} else {
+		if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
+			delete _baro_buffer;
+			_baro_buffer = nullptr;
+			printBufferAllocationFailed("baro");
 			return;
 		}
 	}
@@ -245,19 +247,18 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 #if defined(CONFIG_EKF2_AIRSPEED)
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_airspeed_buffer == nullptr) {
-		if (_obs_buffer_length > 0) {
-			_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
+		_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);

-			if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
-				delete _airspeed_buffer;
-				_airspeed_buffer = nullptr;
-				printBufferAllocationFailed("airspeed");
-				return;
-			}
-
-		} else {
+		if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
+			delete _airspeed_buffer;
+			_airspeed_buffer = nullptr;
+			printBufferAllocationFailed("airspeed");
 			return;
 		}
 	}
@@ -284,19 +285,18 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		if (_obs_buffer_length > 0) {
-			_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
+		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);

-			if (_range_buffer == nullptr || !_range_buffer->valid()) {
-				delete _range_buffer;
-				_range_buffer = nullptr;
-				printBufferAllocationFailed("range");
-				return;
-			}
-
-		} else {
+		if (_range_buffer == nullptr || !_range_buffer->valid()) {
+			delete _range_buffer;
+			_range_buffer = nullptr;
+			printBufferAllocationFailed("range");
 			return;
 		}
 	}
@@ -324,19 +324,18 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_flow_buffer == nullptr) {
-		if (_imu_buffer_length > 0) {
-			_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
+		_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);

-			if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
-				delete _flow_buffer;
-				_flow_buffer = nullptr;
-				printBufferAllocationFailed("flow");
-				return;
-			}
-
-		} else {
+		if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
+			delete _flow_buffer;
+			_flow_buffer = nullptr;
+			printBufferAllocationFailed("flow");
 			return;
 		}
 	}
@@ -363,19 +362,18 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_ext_vision_buffer == nullptr) {
-		if (_obs_buffer_length > 0) {
-			_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
+		_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);

-			if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
-				delete _ext_vision_buffer;
-				_ext_vision_buffer = nullptr;
-				printBufferAllocationFailed("vision");
-				return;
-			}
-
-		} else {
+		if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
+			delete _ext_vision_buffer;
+			_ext_vision_buffer = nullptr;
+			printBufferAllocationFailed("vision");
 			return;
 		}
 	}
@@ -404,19 +402,18 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 #if defined(CONFIG_EKF2_AUXVEL)
 void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_auxvel_buffer == nullptr) {
-		if (_obs_buffer_length > 0) {
-			_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
+		_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);

-			if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
-				delete _auxvel_buffer;
-				_auxvel_buffer = nullptr;
-				printBufferAllocationFailed("aux vel");
-				return;
-			}
-
-		} else {
+		if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
+			delete _auxvel_buffer;
+			_auxvel_buffer = nullptr;
+			printBufferAllocationFailed("aux vel");
 			return;
 		}
 	}
@@ -442,19 +439,18 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)

 void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 {
+	if (!_initialised) {
+		return;
+	}
+
 	// Allocate the required buffer size if not previously done
 	if (_system_flag_buffer == nullptr) {
-		if (_obs_buffer_length > 0) {
-			_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);
+		_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);

-			if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
-				delete _system_flag_buffer;
-				_system_flag_buffer = nullptr;
-				printBufferAllocationFailed("system flag");
-				return;
-			}
-
-		} else {
+		if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
+			delete _system_flag_buffer;
+			_system_flag_buffer = nullptr;
+			printBufferAllocationFailed("system flag");
 			return;
 		}
 	}
@@ -481,21 +477,16 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
 	// sufficient samples have been collected
-	if (_params.drag_ctrl) {
+	if (_params.drag_ctrl > 0) {

 		// Allocate the required buffer size if not previously done
 		if (_drag_buffer == nullptr) {
-			if (_obs_buffer_length > 0) {
-				_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
+			_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);

-				if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
-					delete _drag_buffer;
-					_drag_buffer = nullptr;
-					printBufferAllocationFailed("drag");
-					return;
-				}
-
-			} else {
+			if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
+				delete _drag_buffer;
+				_drag_buffer = nullptr;
+				printBufferAllocationFailed("drag");
 				return;
 			}
 		}
@@ -545,7 +536,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 }
 #endif // CONFIG_EKF2_DRAG_FUSION

-bool EstimatorInterface::initialise_interface()
+bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
 	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;

@@ -569,6 +560,11 @@ bool EstimatorInterface::initialise_interface()
 		return false;
 	}

+	_time_delayed_us = timestamp;
+	_time_latest_us = timestamp;
+
+	_fault_status.value = 0;
+
 	return true;
 }

@@ -318,7 +318,9 @@ public:
 protected:

 	EstimatorInterface() = default;
-	~EstimatorInterface();
+	virtual ~EstimatorInterface();
+
+	virtual bool init(uint64_t timestamp) = 0;

 	parameters _params{};		// filter parameters

@@ -344,11 +346,6 @@ protected:
 	uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon
 	uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data

-	// Variables used by the initial filter alignment
-	bool _is_first_imu_sample{true};
-	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
-	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
-
 	OutputPredictor _output_predictor{};

 #if defined(CONFIG_EKF2_AIRSPEED)
@@ -381,6 +378,7 @@ protected:
 	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};		// air density (kg/m**3)

 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
+	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized

 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	MapProjection _local_origin_lat_lon{};
@@ -451,7 +449,7 @@ protected:
 	fault_status_u _fault_status{};

 	// allocate data buffers and initialize interface variables
-	bool initialise_interface();
+	bool initialise_interface(uint64_t timestamp);

 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control
@@ -337,7 +337,9 @@ def predict_sideslip(
    vel_rel = state["vel"] - wind
    relative_wind_body = state["quat_nominal"].inverse() * vel_rel

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

    return sideslip_pred

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

  // Input arrays

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

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

    _h.setZero();

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

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

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

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

  // Input arrays

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

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

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

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

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

@@ -833,14 +833,6 @@ void EKF2::Run()
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 			UpdateMagCalibration(now);
 #endif // CONFIG_EKF2_MAGNETOMETER
-
-		} else {
-			// if filter not initialized
-
-
-
-
-
 		}

 		// publish ekf2_timestamps
@@ -1,6 +1,6 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
-90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
+10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
+90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
 190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,0,0,-1.2e+02,-3e-11,-2.6e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082
 290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,0,0,-1.2e+02,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11
 390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0,0,-1.2e+02,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13
@@ -1,6 +1,6 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
-90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
+10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
+90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
 190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.035,0,0,-4.9e+02,1.6e-09,-1.4e-09,-6.4e-11,0,0,-3.2e-06,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082
 290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.038,0,0,-4.9e+02,7.3e-09,-1.1e-08,-3.9e-10,0,0,-2e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11
 390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.05,0,0,-4.9e+02,-2.1e-10,-1.4e-08,-3.2e-10,0,0,-1.4e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13
@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -54,7 +54,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_ekf->set_is_fixed_wing(false);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}
@@ -56,7 +56,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -65,7 +65,7 @@ public:
 		_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);

 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -60,7 +60,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}
@@ -58,7 +58,7 @@ public:
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
-		_ekf->reset();
+		_ekf->init(0);
 		_ekf_wrapper.disableBaroHeightFusion();
 		_ekf_wrapper.disableRangeHeightFusion();
 		_sensor_simulator.runSeconds(0.1);
@@ -47,7 +47,8 @@ public:
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
-		_ekf.reset();
+		_ekf.init(0);
+
 	}

 	void TearDown() override
@@ -56,7 +56,7 @@ public:
 	void SetUp() override
 	{
 		// first init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}
@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}
@@ -45,7 +45,7 @@ public:

 	void SetUp() override
 	{
-		_ekf->reset();
+		_ekf->init(0);
 	}
 };

@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->reset();
+		_ekf->init(0);
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -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
@@ -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
@@ -35,5 +35,5 @@ px4_add_library(RoverAckermannControl
 	RoverAckermannControl.cpp
 )

-target_link_libraries(RoverAckermannControl PUBLIC PID)
+target_link_libraries(RoverAckermannControl PUBLIC pid)
 target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -41,19 +41,27 @@ RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParam
 {
 	updateParams();
 	_rover_ackermann_status_pub.advertise();
+	pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
+
 }

 void RoverAckermannControl::updateParams()
 {
 	ModuleParams::updateParams();

-	_pid_throttle.setGains(_param_ra_speed_p.get(), _param_ra_speed_i.get(), 0.f);
-	_pid_throttle.setIntegralLimit(1.f);
-	_pid_throttle.setOutputLimit(1.f);
+	pid_set_parameters(&_pid_throttle,
+			   _param_ra_speed_p.get(), // Proportional gain
+			   _param_ra_speed_i.get(), // Integral gain
+			   0, // Derivative gain
+			   _param_ra_speed_i.get() > FLT_EPSILON ? 1.f / _param_ra_speed_i.get() : 0.f, // Integral limit
+			   1); // Output limit

-	_pid_lat_accel.setGains(_param_ra_lat_accel_p.get(), _param_ra_lat_accel_i.get(), 0.f);
-	_pid_lat_accel.setIntegralLimit(1.f);
-	_pid_lat_accel.setOutputLimit(1.f);
+	pid_set_parameters(&_pid_lat_accel,
+			   _param_ra_lat_accel_p.get(), // Proportional gain
+			   _param_ra_lat_accel_i.get(), // Integral gain
+			   0, // Derivative gain
+			   _param_ra_lat_accel_i.get() > FLT_EPSILON ? 1.f / _param_ra_lat_accel_i.get() : 0.f, // Integral limit
+			   1); // Output limit

 	// Update slew rates
 	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
@@ -109,8 +117,8 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe

 			if (sign(vehicle_forward_speed_temp) ==
 			    1) { // Only do closed loop control when driving forwards (backwards driving is non-minimum phase and can therefor introduce instability)
-				_pid_lat_accel.setSetpoint(_rover_ackermann_setpoint.lateral_acceleration_setpoint);
-				steering_setpoint += _pid_lat_accel.update(vehicle_lateral_acceleration, dt);
+				steering_setpoint += pid_calculate(&_pid_lat_accel, _rover_ackermann_setpoint.lateral_acceleration_setpoint,
+								   vehicle_lateral_acceleration, 0, dt);
 			}

 			_rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(),
@@ -148,8 +156,8 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe
 	_rover_ackermann_status.steering_setpoint_normalized = steering_normalized;
 	_rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState();
 	_rover_ackermann_status.measured_lateral_acceleration = vehicle_lateral_acceleration;
-	_rover_ackermann_status.pid_throttle_integral = _pid_throttle.getIntegral();
-	_rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.getIntegral();
+	_rover_ackermann_status.pid_throttle_integral = _pid_throttle.integral *  _param_ra_speed_i.get();
+	_rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.integral * _param_ra_lat_accel_i.get();
 	_rover_ackermann_status_pub.publish(_rover_ackermann_status);

 	// Publish to motor
@@ -210,8 +218,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe
 						   -1.f, 1.f);
 		}

-		_pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState());
-		forward_speed_normalized += _pid_throttle.update(vehicle_forward_speed, dt);
+		forward_speed_normalized += pid_calculate(&_pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
+					    vehicle_forward_speed, 0, dt); // Feedback
 	}

 	return math::constrain(forward_speed_normalized, -1.f, 1.f);
@@ -220,8 +228,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe

 void RoverAckermannControl::resetControllers()
 {
-	_pid_throttle.resetIntegral();
-	_pid_lat_accel.resetIntegral();
+	pid_reset_integral(&_pid_throttle);
+	pid_reset_integral(&_pid_lat_accel);
 	_forward_speed_setpoint_with_accel_limit.setForcedValue(0.f);
 	_steering_with_rate_limit.setForcedValue(0.f);

@@ -46,7 +46,7 @@
 #include <uORB/topics/actuator_servos.h>

 // Standard libraries
-#include <lib/pid/PID.hpp>
+#include <lib/pid/pid.h>
 #include <matrix/matrix/math.hpp>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -114,8 +114,8 @@ private:
 	hrt_abstime _timestamp{0};

 	// Controllers
-	PID _pid_throttle; // The PID controller for the closed loop speed control
-	PID _pid_lat_accel; // The PID controller for the closed loop speed control
+	PID_t _pid_throttle; // The PID controller for the closed loop speed control
+	PID_t _pid_lat_accel; // The PID controller for the closed loop speed control
 	SlewRate<float> _steering_with_rate_limit{0.f};
 	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};

@@ -57,7 +57,7 @@
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <math.h>
-#include <lib/pid/PID.hpp>
+#include <lib/pid/pid.h>

 using namespace matrix;

@@ -35,5 +35,5 @@ px4_add_library(RoverDifferentialControl
 	RoverDifferentialControl.cpp
 )

-target_link_libraries(RoverDifferentialControl PUBLIC PID)
+target_link_libraries(RoverDifferentialControl PUBLIC pid)
 target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -42,6 +42,9 @@ RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : Modul
 {
 	updateParams();
 	_rover_differential_status_pub.advertise();
+	pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f);
 }

 void RoverDifferentialControl::updateParams()
@@ -51,15 +54,24 @@ void RoverDifferentialControl::updateParams()
 	_max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F;

 	// Update PID
-	_pid_yaw_rate.setGains(_param_rd_yaw_rate_p.get(), _param_rd_yaw_rate_i.get(), 0.f);
-	_pid_yaw_rate.setIntegralLimit(1.f);
-	_pid_yaw_rate.setOutputLimit(1.f);
-	_pid_throttle.setGains(_param_rd_p_gain_speed.get(), _param_rd_i_gain_speed.get(), 0.f);
-	_pid_throttle.setIntegralLimit(1.f);
-	_pid_throttle.setOutputLimit(1.f);
-	_pid_yaw.setGains(_param_rd_p_gain_yaw.get(), _param_rd_i_gain_yaw.get(), 0.f);
-	_pid_yaw.setIntegralLimit(_max_yaw_rate);
-	_pid_yaw.setOutputLimit(_max_yaw_rate);
+	pid_set_parameters(&_pid_yaw_rate,
+			   _param_rd_yaw_rate_p.get(), // Proportional gain
+			   _param_rd_yaw_rate_i.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
+	pid_set_parameters(&_pid_throttle,
+			   _param_rd_p_gain_speed.get(), // Proportional gain
+			   _param_rd_i_gain_speed.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
+	pid_set_parameters(&_pid_yaw,
+			   _param_rd_p_gain_yaw.get(),  // Proportional gain
+			   _param_rd_i_gain_yaw.get(),  // Integral gain
+			   0.f,  // Derivative gain
+			   _max_yaw_rate,  // Integral limit
+			   _max_yaw_rate);  // Output limit

 	// Update slew rates
 	if (_max_yaw_rate > FLT_EPSILON) {
@@ -87,10 +99,8 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
 		_yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt);
 		_rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState());
-		_pid_yaw.setSetpoint(
-			matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() -
-					vehicle_yaw));  // error as setpoint to take care of wrapping
-		_rover_differential_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt);
+		const float heading_error = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - vehicle_yaw);
+		_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
 		_rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint;

 	} else {
@@ -132,9 +142,9 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	_rover_differential_status.measured_forward_speed = vehicle_forward_speed;
 	_rover_differential_status.measured_yaw = vehicle_yaw;
 	_rover_differential_status.measured_yaw_rate = vehicle_yaw_rate;
-	_rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
-	_rover_differential_status.pid_throttle_integral = _pid_throttle.getIntegral();
-	_rover_differential_status.pid_yaw_integral = _pid_yaw.getIntegral();
+	_rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
+	_rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
+	_rover_differential_status.pid_yaw_integral = _pid_yaw.integral;
 	_rover_differential_status_pub.publish(_rover_differential_status);

 	// Publish to motors
@@ -148,7 +158,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate,
 		const float max_thr_yaw_r,
 		const float max_yaw_accel, const float wheel_track, const float dt, SlewRate<float> &yaw_rate_with_accel_limit,
-		PID &pid_yaw_rate, const bool normalized)
+		PID_t &pid_yaw_rate, const bool normalized)
 {
 	float slew_rate_normalization{1.f};

@@ -180,8 +190,8 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set
 						max_thr_yaw_r, -1.f, 1.f);
 		}

-		pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState());
-		speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt);
+		speed_diff_normalized += pid_calculate(&pid_yaw_rate, yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0,
+						       dt); // Feedback
 	}

 	return math::constrain(speed_diff_normalized, -1.f, 1.f);
@@ -190,7 +200,7 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set

 float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint,
 		const float vehicle_forward_speed, const float max_thr_spd, SlewRate<float> &forward_speed_setpoint_with_accel_limit,
-		PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized)
+		PID_t &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized)
 {
 	float slew_rate_normalization{1.f};

@@ -232,8 +242,8 @@ float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_
 						   -1.f, 1.f);
 		}

-		pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState());
-		forward_speed_normalized += pid_throttle.update(vehicle_forward_speed, dt);
+		forward_speed_normalized += pid_calculate(&pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
+					    vehicle_forward_speed, 0, dt); // Feedback
 	}

 	return math::constrain(forward_speed_normalized, -1.f, 1.f);
@@ -257,7 +267,7 @@ matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forwar

 void RoverDifferentialControl::resetControllers()
 {
-	_pid_throttle.resetIntegral();
-	_pid_yaw_rate.resetIntegral();
-	_pid_yaw.resetIntegral();
+	pid_reset_integral(&_pid_throttle);
+	pid_reset_integral(&_pid_yaw_rate);
+	pid_reset_integral(&_pid_yaw);
 }
@@ -47,7 +47,7 @@
 #include <lib/slew_rate/SlewRateYaw.hpp>

 // Standard libraries
-#include <lib/pid/PID.hpp>
+#include <lib/pid/pid.h>
 #include <matrix/matrix/math.hpp>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -100,7 +100,7 @@ private:
 	 * @return Normalized speed differece setpoint with applied slew rates [-1, 1].
 	 */
 	float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel,
-				      float wheel_track, float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized);
+				      float wheel_track, float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID_t &pid_yaw_rate, bool normalized);
 	/**
 	 * @brief Compute normalized forward speed setpoint and apply slew rates.
 	 * to the forward speed setpoint and doing closed loop speed control if enabled.
@@ -116,7 +116,7 @@ private:
 	 * @return Normalized forward speed setpoint with applied slew rates [-1, 1].
 	 */
 	float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd,
-					  SlewRate<float> &forward_speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel,
+					  SlewRate<float> &forward_speed_setpoint_with_accel_limit, PID_t &pid_throttle, float max_accel, float max_decel,
 					  float dt, bool normalized);

 	/**
@@ -142,9 +142,9 @@ private:
 	float _max_yaw_accel{0.f};

 	// Controllers
-	PID _pid_throttle; // Closed loop speed control
-	PID _pid_yaw;      // Closed loop yaw control
-	PID _pid_yaw_rate; // Closed loop yaw rate control
+	PID_t _pid_throttle; // Closed loop speed control
+	PID_t _pid_yaw;      // Closed loop yaw control
+	PID_t _pid_yaw_rate; // Closed loop yaw rate control
 	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
 	SlewRate<float> _yaw_rate_with_accel_limit{0.f};
 	SlewRateYaw<float> _yaw_setpoint_with_yaw_rate_limit;
@@ -53,7 +53,7 @@
 #include <uORB/topics/rover_mecanum_setpoint.h>

 // Standard libraries
-#include <lib/pid/PID.hpp>
+#include <lib/pid/pid.h>
 #include <matrix/matrix/math.hpp>

 // Local includes
@@ -35,5 +35,5 @@ px4_add_library(RoverMecanumControl
 	RoverMecanumControl.cpp
 )

-target_link_libraries(RoverMecanumControl PUBLIC PID)
+target_link_libraries(RoverMecanumControl PUBLIC pid)
 target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -42,28 +42,40 @@ RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(pa
 {
 	updateParams();
 	_rover_mecanum_status_pub.advertise();
+	pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_forward_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f);
 }

 void RoverMecanumControl::updateParams()
 {
 	ModuleParams::updateParams();
-
-	_pid_yaw_rate.setGains(_param_rm_yaw_rate_p.get(), _param_rm_yaw_rate_i.get(), 0.f);
-	_pid_yaw_rate.setIntegralLimit(1.f);
-	_pid_yaw_rate.setOutputLimit(1.f);
-
-	_pid_forward_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f);
-	_pid_forward_throttle.setIntegralLimit(1.f);
-	_pid_forward_throttle.setOutputLimit(1.f);
-
-	_pid_lateral_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f);
-	_pid_lateral_throttle.setIntegralLimit(1.f);
-	_pid_lateral_throttle.setOutputLimit(1.f);
-
 	_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
-	_pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f);
-	_pid_yaw.setIntegralLimit(_max_yaw_rate);
-	_pid_yaw.setOutputLimit(_max_yaw_rate);
+	pid_set_parameters(&_pid_yaw_rate,
+			   _param_rm_yaw_rate_p.get(), // Proportional gain
+			   _param_rm_yaw_rate_i.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
+	pid_set_parameters(&_pid_forward_throttle,
+			   _param_rm_p_gain_speed.get(), // Proportional gain
+			   _param_rm_i_gain_speed.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
+	pid_set_parameters(&_pid_lateral_throttle,
+			   _param_rm_p_gain_speed.get(), // Proportional gain
+			   _param_rm_i_gain_speed.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
+	pid_set_parameters(&_pid_yaw,
+			   _param_rm_p_gain_yaw.get(),  // Proportional gain
+			   _param_rm_i_gain_yaw.get(),  // Integral gain
+			   0.f,  // Derivative gain
+			   _max_yaw_rate,  // Integral limit
+			   _max_yaw_rate);  // Output limit
 }

 void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
@@ -79,12 +91,11 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl

 	// Closed loop yaw control
 	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) {
-		_pid_yaw.setSetpoint(
-			matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw));  // error as setpoint to take care of wrapping
-		_rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt);
+		const float heading_error = matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw);
+		_rover_mecanum_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0.f, 0.f, dt);

 	} else {
-		_pid_yaw.resetIntegral();
+		pid_reset_integral(&_pid_yaw);
 	}

 	// Yaw rate control
@@ -97,9 +108,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 						_param_rm_max_thr_yaw_r.get(), -1.f, 1.f);
 		}

-		_pid_yaw_rate.setSetpoint(_rover_mecanum_setpoint.yaw_rate_setpoint);
 		speed_diff_normalized = math::constrain(speed_diff_normalized +
-							_pid_yaw_rate.update(vehicle_yaw_rate, dt),
+							pid_calculate(&_pid_yaw_rate, _rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0.f, dt),
 							-1.f, 1.f); // Feedback

 	} else { // Use normalized setpoint
@@ -119,8 +129,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 					   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
 		}

-		_pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint);
-		forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt);
+		forward_throttle += pid_calculate(&_pid_forward_throttle, _rover_mecanum_setpoint.forward_speed_setpoint,
+						  vehicle_forward_speed, 0, dt); // Feedback

 		// Closed loop lateral speed control
 		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
@@ -128,8 +138,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 					   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
 		}

-		_pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint);
-		lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt);
+		lateral_throttle += pid_calculate(&_pid_lateral_throttle, _rover_mecanum_setpoint.lateral_speed_setpoint,
+						  vehicle_lateral_speed, 0, dt); // Feedback

 	} else { // Use normalized setpoint
 		forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain(
@@ -146,10 +156,10 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 	rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint;
 	rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate;
 	rover_mecanum_status.measured_yaw = vehicle_yaw;
-	rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
-	rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral();
-	rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral();
-	rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral();
+	rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
+	rover_mecanum_status.pid_yaw_integral = _pid_yaw.integral;
+	rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.integral;
+	rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.integral;
 	_rover_mecanum_status_pub.publish(rover_mecanum_status);

 	// Publish to motors
@@ -203,8 +213,8 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr

 void RoverMecanumControl::resetControllers()
 {
-	_pid_forward_throttle.resetIntegral();
-	_pid_lateral_throttle.resetIntegral();
-	_pid_yaw_rate.resetIntegral();
-	_pid_yaw.resetIntegral();
+	pid_reset_integral(&_pid_forward_throttle);
+	pid_reset_integral(&_pid_lateral_throttle);
+	pid_reset_integral(&_pid_yaw_rate);
+	pid_reset_integral(&_pid_yaw);
 }
@@ -46,7 +46,7 @@


 // Standard libraries
-#include <lib/pid/PID.hpp>
+#include <lib/pid/pid.h>
 #include <matrix/matrix/math.hpp>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -112,10 +112,10 @@ private:
 	float _max_yaw_rate{0.f};

 	// Controllers
-	PID _pid_forward_throttle; // PID for the closed loop forward speed control
-	PID _pid_lateral_throttle; // PID for the closed loop lateral speed control
-	PID _pid_yaw; // PID for the closed loop yaw control
-	PID _pid_yaw_rate; // PID for the closed loop yaw rate control
+	PID_t _pid_forward_throttle; // PID for the closed loop forward speed control
+	PID_t _pid_lateral_throttle; // PID for the closed loop lateral speed control
+	PID_t _pid_yaw; // PID for the closed loop yaw control
+	PID_t _pid_yaw_rate; // PID for the closed loop yaw rate control

 	// Parameters
 	DEFINE_PARAMETERS(
@@ -38,5 +38,5 @@ px4_add_module(
 		RoverPositionControl.hpp
 	DEPENDS
 		l1
-		PID
+		pid
 	)
@@ -92,9 +92,13 @@ void RoverPositionControl::parameters_update(bool force)
 		_gnd_control.set_l1_damping(_param_l1_damping.get());
 		_gnd_control.set_l1_period(_param_l1_period.get());

-		_speed_ctrl.setGains(_param_speed_p.get(), _param_speed_i.get(), _param_speed_d.get());
-		_speed_ctrl.setIntegralLimit(_param_speed_imax.get());
-		_speed_ctrl.setOutputLimit(_param_gndspeed_max.get());
+		pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f);
+		pid_set_parameters(&_speed_ctrl,
+				   _param_speed_p.get(),
+				   _param_speed_i.get(),
+				   _param_speed_d.get(),
+				   _param_speed_imax.get(),
+				   _param_gndspeed_max.get());
 	}
 }

@@ -235,9 +239,12 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 			const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 			const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));

+			const float x_vel = vel(0);
+			const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
+
 			// Compute airspeed control out and just scale it as a constant
-			_speed_ctrl.setSetpoint(mission_target_speed);
-			mission_throttle = _param_throttle_speed_scaler.get() * _speed_ctrl.update(vel(0), dt);
+			mission_throttle = _param_throttle_speed_scaler.get()
+					   * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);

 			// Constrain throttle between min and max
 			mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get());
@@ -320,8 +327,10 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
 		const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 		const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2));

-		_speed_ctrl.setSetpoint(desired_speed);
-		const float control_throttle = _speed_ctrl.update(vel(0), dt);
+		const float x_vel = vel(0);
+		const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
+
+		const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt);

 		//Constrain maximum throttle to mission throttle
 		_throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle);
@@ -383,6 +392,8 @@ RoverPositionControl::Run()
 		vehicle_attitude_poll();
 		manual_control_setpoint_poll();

+		_vehicle_acceleration_sub.update();
+
 		/* update parameters from storage */
 		parameters_update();

@@ -48,7 +48,7 @@
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/pid/PID.hpp>
+#include <lib/pid/pid.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
@@ -128,6 +128,8 @@ private:
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};

+	uORB::SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+
 	perf_counter_t	_loop_perf;			/**< loop performance counter */

 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
@@ -137,7 +139,7 @@ private:

 	/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
 	 the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
-	PID _speed_ctrl{};
+	PID_t _speed_ctrl{};

 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};		// captures the number of times the estimator has reset the horizontal position
@@ -327,9 +327,9 @@ void Sih::generate_fw_aerodynamics()
 {
 	_v_B = _C_IB.transpose() * _v_I; 	// velocity in body frame [m/s]
 	float altitude = _H0 - _p_I(2);
-	_wing_l.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
-	_wing_r.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX);
-	_tailplane.update_aero(_v_B, _w_B, altitude, -_u[1]*FLAP_MAX, _T_MAX * _u[3]);
+	_wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX);
+	_wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
+	_tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]);
 	_fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]);
 	_fuselage.update_aero(_v_B, _w_B, altitude);

@@ -409,9 +409,11 @@ void Sih::equations_of_motion(const float dt)
 			// integration: Euler forward
 			_p_I = _p_I + _p_I_dot * dt;
 			_v_I = _v_I + _v_I_dot * dt;
-			_q = _q * _dq;
-			_q.normalize();
-			_w_B = constrain(_w_B + _w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F);
+			Eulerf RPY = Eulerf(_q);
+			RPY(0) = 0.0f;	// no roll
+			RPY(1) = radians(0.0f); // pitch slightly up if needed to get some lift
+			_q = Quatf(RPY);
+			_w_B.setZero();
 			_grounded = true;
 		}

@@ -448,7 +450,8 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
 	// TODO: send differential pressure instead?
 	airspeed_s airspeed{};
 	airspeed.timestamp_sample = time_now_us;
-	airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B.norm() + generate_wgn() * 0.2f);
+	// airspeed sensor is mounted along the negative Z axis since the vehicle is a tailsitter
+	airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f);
 	airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
 	airspeed.air_temperature_celsius = NAN;
 	airspeed.confidence = 0.7f;
@@ -49,6 +49,7 @@
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
+#include <lib/pid/pid.h>
 #include <matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -48,6 +48,7 @@
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
+#include <lib/pid/pid.h>
 #include <matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -31,7 +31,7 @@
 #
 ############################################################################

-if(${CMAKE_VERSION} VERSION_LESS_EQUAL "3.15")
+if(${CMAKE_VERSION} VERSION_LESS "3.11")
 	message(WARNING "skipping uxrce_dds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}")

 else()
@@ -116,7 +116,7 @@ else()


 	add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h
-		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
+		COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
 			--topic-msg-dir ${PX4_SOURCE_DIR}/msg
 			--client-outdir ${CMAKE_CURRENT_BINARY_DIR}
 			--dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml