cmake: move FindPythonInterp->FindPython

- cmake CMP0148 FindPythonInterp deprecated
2026-05-23 21:07:35 +08:00 · 2024-11-22 21:57:53 -05:00
99 changed files with 746 additions and 703 deletions
@@ -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)
@@ -241,24 +242,19 @@ if(NOT CMAKE_BUILD_TYPE)
 endif()

 if((CMAKE_BUILD_TYPE STREQUAL "Debug") OR (CMAKE_BUILD_TYPE STREQUAL "Coverage"))
-	set(MAX_CUSTOM_OPT_LEVEL_SPEED -O0)
+	set(MAX_CUSTOM_OPT_LEVEL -O0)
 elseif(CMAKE_BUILD_TYPE MATCHES "Sanitizer")
-	set(MAX_CUSTOM_OPT_LEVEL_SPEED -O1)
+	set(MAX_CUSTOM_OPT_LEVEL -O1)
 elseif(CMAKE_BUILD_TYPE MATCHES "Release")
-	set(MAX_CUSTOM_OPT_LEVEL_SPEED -O3)
+	set(MAX_CUSTOM_OPT_LEVEL -O3)
 else()
 	if(px4_constrained_flash_build)
-		set(MAX_CUSTOM_OPT_LEVEL_SPEED -Os)
+		set(MAX_CUSTOM_OPT_LEVEL -Os)
 	else()
-		set(MAX_CUSTOM_OPT_LEVEL_SPEED -O2)
-		set(MAX_CUSTOM_OPT_LEVEL_SPACE -Os)
+		set(MAX_CUSTOM_OPT_LEVEL -O2)
 	endif()
 endif()

-if(NOT MAX_CUSTOM_OPT_LEVEL_SPACE)
-	set(MAX_CUSTOM_OPT_LEVEL_SPACE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
-endif()
-
 set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage;AddressSanitizer;UndefinedBehaviorSanitizer")
 message(STATUS "cmake build type: ${CMAKE_BUILD_TYPE}")

@@ -486,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
@@ -507,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
@@ -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/
@@ -43,4 +43,4 @@ if(PX4_TESTING)
 	add_subdirectory(test)
 endif()

-target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -80,7 +80,7 @@ if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
 		${SRCS_KERNEL}
 		)
 	target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs nuttx_mm heatshrink)
-	target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED} -D__KERNEL__)
+	target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__)

 	# User side library in nuttx kernel/protected build
 	px4_add_library(uORB
@@ -106,7 +106,7 @@ else()
 endif()

 target_link_libraries(uORB PRIVATE uorb_msgs heatshrink)
-target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 if(PX4_TESTING)
 	add_subdirectory(uORB_tests)
@@ -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
@@ -34,4 +34,4 @@
 px4_add_library(arch_dshot
 	dshot.c
 )
-target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -34,6 +34,6 @@
 px4_add_library(arch_spi
 	spi.cpp
 )
-target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 target_link_libraries (arch_spi PRIVATE drivers_board) # px4_spi_buses
@@ -36,6 +36,6 @@ px4_add_library(arch_hrt
 )
 target_compile_options(arch_hrt
 	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 		-Wno-cast-align # TODO: fix and enable
 )
@@ -39,6 +39,6 @@ px4_add_library(arch_io_pins
 	rp2040_pinset.c
 )

-target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 target_link_libraries(arch_io_pins PRIVATE drivers_board)
@@ -34,4 +34,4 @@
 px4_add_library(arch_spi
 	spi.cpp
 )
-target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -34,4 +34,4 @@
 px4_add_library(arch_dshot
 	dshot.c
 )
-target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -36,6 +36,6 @@ px4_add_library(arch_hrt
 )
 target_compile_options(arch_hrt
 	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 		-Wno-cast-align # TODO: fix and enable
 )
@@ -37,6 +37,6 @@ px4_add_library(arch_io_pins
 	pwm_servo.c
 	pwm_trigger.c
 )
-target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 target_link_libraries(arch_io_pins PRIVATE drivers_board) # timer_io_channels
@@ -34,6 +34,6 @@
 px4_add_library(arch_spi
 	spi.cpp
 )
-target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 target_link_libraries (arch_spi PRIVATE drivers_board) # px4_spi_buses
@@ -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
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE drivers__imu__analog_devices__adis16448
 	MAIN adis16448
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPACE}
+		${MAX_CUSTOM_OPT_LEVEL}
 		#-DDEBUG_BUILD
 	SRCS
 		ADIS16448.cpp
@@ -34,7 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__icm42688p
 	MAIN icm42688p
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPACE}
+		${MAX_CUSTOM_OPT_LEVEL}
 		#-DDEBUG_BUILD
 	SRCS
 		icm42688p_main.cpp
@@ -34,7 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__icm45686
 	MAIN icm45686
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPACE}
+		${MAX_CUSTOM_OPT_LEVEL}
 		#-DDEBUG_BUILD
 	SRCS
 		icm45686_main.cpp
@@ -34,7 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__iim42652
 	MAIN iim42652
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPACE}
+		${MAX_CUSTOM_OPT_LEVEL}
 		#-DDEBUG_BUILD
 	SRCS
 		iim42652_main.cpp
@@ -34,7 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__iim42653
 	MAIN iim42653
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPACE}
+		${MAX_CUSTOM_OPT_LEVEL}
 		#-DDEBUG_BUILD
 	SRCS
 		iim42653_main.cpp
@@ -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
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__fake_imu
 	MAIN fake_imu
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPACE}
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		FakeImu.cpp
 		FakeImu.hpp
@@ -49,7 +49,7 @@ px4_add_library(cdev
 	CDev.hpp
 	${SRCS_PLATFORM}
 )
-target_compile_options(cdev PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(cdev PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 if (${PX4_PLATFORM} STREQUAL "nuttx")
 	target_link_libraries(cdev PRIVATE nuttx_fs) # register/unregister driver
@@ -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}
@@ -36,6 +36,6 @@ px4_add_library(conversion
 	rotation.h
 )

-target_compile_options(conversion PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(conversion PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 px4_add_unit_gtest(SRC RotationTest.cpp LINKLIBS conversion)
@@ -36,4 +36,4 @@ add_library(crc STATIC EXCLUDE_FROM_ALL
 	crc.h
 )
 add_dependencies(crc prebuild_targets)
-target_compile_options(crc PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(crc PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -35,5 +35,5 @@ px4_add_library(drivers_accelerometer
 	PX4Accelerometer.cpp
 	PX4Accelerometer.hpp
 )
-target_compile_options(drivers_accelerometer PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPACE})
+target_compile_options(drivers_accelerometer PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(drivers_accelerometer PRIVATE conversion)
@@ -35,5 +35,5 @@ px4_add_library(drivers_gyroscope
 	PX4Gyroscope.cpp
 	PX4Gyroscope.hpp
 )
-target_compile_options(drivers_gyroscope PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPACE})
+target_compile_options(drivers_gyroscope PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(drivers_gyroscope PRIVATE conversion)
@@ -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}
@@ -36,6 +36,6 @@ add_library(geo
 	geo.h
 )
 add_dependencies(geo prebuild_targets)
-target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 px4_add_unit_gtest(SRC test_geo.cpp LINKLIBS geo)
@@ -38,5 +38,5 @@ px4_add_library(heatshrink
 )

 target_compile_options(heatshrink PRIVATE
-	${MAX_CUSTOM_OPT_LEVEL_SPEED}
+	${MAX_CUSTOM_OPT_LEVEL}
 	-DHEATSHRINK_DYNAMIC_ALLOC=0)
@@ -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
@@ -61,7 +61,7 @@ px4_add_library(mixer_module
 	)

 add_dependencies(mixer_module output_functions_header)
-target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(mixer_module PRIVATE ${CMAKE_CURRENT_BINARY_DIR})

 px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module)
@@ -31,7 +31,7 @@
 #
 ############################################################################

-add_compile_options(${MAX_CUSTOM_OPT_LEVEL_SPEED})
+add_compile_options(${MAX_CUSTOM_OPT_LEVEL})

 if (NOT PARAM_DEFAULT_OVERRIDES)
 	set(PARAM_DEFAULT_OVERRIDES "{}")
@@ -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
@@ -33,4 +33,4 @@

 add_library(perf perf_counter.cpp)
 add_dependencies(perf prebuild_targets)
-target_compile_options(perf PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(perf PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -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_ */
@@ -35,7 +35,7 @@ px4_add_library(RateControl
 	rate_control.cpp
 	rate_control.hpp
 )
-target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(RateControl PRIVATE mathlib)

@@ -47,4 +47,4 @@ if(${PX4_PLATFORM} STREQUAL "nuttx")
 endif()

 px4_add_library(systemlib ${SRCS})
-target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -39,7 +39,7 @@ target_compile_definitions(tinybson PRIVATE -DMODULE_NAME="tinybson")
 target_compile_options(tinybson
 	PRIVATE
 		-Wno-sign-compare # TODO: fix this
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 )

 add_dependencies(tinybson prebuild_targets)
@@ -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
@@ -53,7 +53,7 @@ add_library(world_magnetic_model
 	geo_magnetic_tables.hpp
 )
 add_dependencies(world_magnetic_model prebuild_targets)
-target_compile_options(world_magnetic_model PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(world_magnetic_model PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 if(BUILD_TESTING)
 	px4_add_unit_gtest(SRC test_geo_lookup.cpp LINKLIBS world_magnetic_model)
@@ -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
@@ -64,7 +64,7 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessRoverAckermann.cpp
 )

-target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE
@@ -39,7 +39,7 @@ px4_add_module(
 	MODULE modules__control_allocator
 	MAIN control_allocator
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 	STACK_MAIN
 		3000
 	SRCS
@@ -39,7 +39,7 @@ px4_add_library(ControlAllocation
 	ControlAllocationSequentialDesaturation.cpp
 	ControlAllocationSequentialDesaturation.hpp
 )
-target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(ControlAllocation PRIVATE mathlib)

@@ -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
@@ -238,7 +238,7 @@ px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 		-fno-associative-math
 		#-DDEBUG_BUILD
 		#-O0
@@ -38,6 +38,6 @@ add_library(lat_lon_alt

 add_dependencies(lat_lon_alt prebuild_targets)
 target_include_directories(lat_lon_alt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_compile_options(lat_lon_alt PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(lat_lon_alt PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS lat_lon_alt geo)
@@ -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)

@@ -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
@@ -35,7 +35,7 @@ px4_add_library(follow_target_estimator
 	TargetEstimator.cpp
 )

-target_compile_options(follow_target_estimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(follow_target_estimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 target_link_libraries(follow_target_estimator
 	PRIVATE
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE fw_autotune_attitude_control
 	MAIN fw_autotune_attitude_control
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		fw_autotune_attitude_control.cpp
 		fw_autotune_attitude_control.hpp
@@ -51,7 +51,7 @@ px4_add_module(
 	STACK_MAIN
 		4096
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 		-DARM_ALL_FFT_TABLES
 		-DARM_MATH_LOOPUNROLL
 	INCLUDES
@@ -37,7 +37,7 @@ px4_add_module(
 	PRIORITY "SCHED_PRIORITY_MAX-30"
 	STACK_MAIN 2500
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 		-Wno-cast-align # TODO: fix and enable
 	SRCS
 		logged_topics.cpp
@@ -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
@@ -36,7 +36,7 @@ px4_add_library(AttitudeControl
 	AttitudeControl.hpp
 	AttitudeControlMath.hpp
 )
-target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
+target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(AttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

 px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl)
@@ -37,7 +37,7 @@ px4_add_module(
 	MODULE modules__mc_att_control
 	MAIN mc_att_control
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		mc_att_control_main.cpp
 		mc_att_control.hpp
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE mc_autotune_attitude_control
 	MAIN mc_autotune_attitude_control
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		mc_autotune_attitude_control.cpp
 		mc_autotune_attitude_control.hpp
@@ -40,7 +40,7 @@ px4_add_module(
 	MODULE modules__mc_hover_thrust_estimator
 	MAIN mc_hover_thrust_estimator
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		MulticopterHoverThrustEstimator.cpp
 		MulticopterHoverThrustEstimator.hpp
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__mc_rate_control
 	MAIN mc_rate_control
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		MulticopterRateControl.cpp
 		MulticopterRateControl.hpp
@@ -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
@@ -36,7 +36,7 @@ px4_add_library(vehicle_acceleration
 	VehicleAcceleration.hpp
 )

-target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPACE})
+target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

 target_link_libraries(vehicle_acceleration
 	PRIVATE
@@ -38,7 +38,7 @@ px4_add_library(vehicle_angular_velocity

 target_compile_options(vehicle_angular_velocity
 	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL_SPACE}
+		${MAX_CUSTOM_OPT_LEVEL}
 		#-DDEBUG_BUILD
 )

@@ -38,7 +38,7 @@ px4_add_library(vehicle_imu

 target_compile_options(vehicle_imu
 	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL_SPACE}
+		${MAX_CUSTOM_OPT_LEVEL}
 		#-DDEBUG_BUILD
 )

@@ -51,7 +51,7 @@ if(gz-transport_FOUND)
 		MODULE modules__simulation__gz_bridge
 		MAIN gz_bridge
 		COMPILE_FLAGS
-			${MAX_CUSTOM_OPT_LEVEL_SPEED}
+			${MAX_CUSTOM_OPT_LEVEL}
 		SRCS
 			GZBridge.cpp
 			GZBridge.hpp
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__simulation__simulator_sih
 	MAIN simulator_sih
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL_SPEED}
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		aero.hpp
 		sih.cpp
@@ -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>
@@ -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
@@ -138,7 +138,7 @@ else()
 		COMPILE_FLAGS
 			#-O0
 			# -DDEBUG_BUILD
-			${MAX_CUSTOM_OPT_LEVEL_SPACE}
+			${MAX_CUSTOM_OPT_LEVEL}
 		SRCS
 			${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h
 			uxrce_dds_client.cpp