switch uxrce to space

Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com>

.github/workflows/dev_container.yml

@@ -25,6 +25,12 @@ jobs:
           submodules: false
           fetch-depth: 0
 
+      - name: Login to Docker Hub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+
       - name: Login to GitHub Container Registry
         uses: docker/login-action@v3
         with:
@@ -38,12 +44,14 @@ jobs:
         with:
           images: |
             ghcr.io/PX4/px4-dev
+            px4io/px4-dev
           tags: |
             type=schedule
             type=semver,pattern={{version}}
             type=semver,pattern={{major}}.{{minor}}
             type=semver,pattern={{major}}
-            type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}}
+            type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}},priority=600
+            type=ref,event=branch,suffix=,priority=500
             type=ref,event=pr
             type=sha
 
@@ -82,7 +90,6 @@ jobs:
 
       - name: Push container image
         uses: docker/build-push-action@v6
-        if: ${{ github.event_name == 'push' }}
         with:
           context: Tools/setup
           tags: ${{ steps.meta.outputs.tags }}

.github/workflows/flash_analysis.yml

@@ -10,8 +10,8 @@ on:
 
 jobs:
   analyze_flash:
-    name: FLASH usage analysis
-    runs-on: ubuntu-latest
+    name: Analyzing ${{ matrix.target }}
+    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
     container:
       image: px4io/px4-dev-nuttx-focal
     strategy:
@@ -29,7 +29,7 @@ jobs:
       - name: Git ownership workaround
         run: git config --system --add safe.directory '*'
 
-      - name: Build
+      - name: Build Target
         run: make ${{ matrix.target }}
 
       - name: Store the ELF with the change
@@ -73,36 +73,46 @@ jobs:
           echo "$EOF" >> $GITHUB_OUTPUT
 
   post_pr_comment:
-    name: Post PR comment
-    runs-on: ubuntu-latest
+    name: Publish Results
+    runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
     needs: [analyze_flash]
+    if: ${{ github.event.pull_request }}
     steps:
-      - name: If it's a PR add a comment with the bloaty output
-        if: ${{ github.event.pull_request }}
-        uses: actions/github-script@v6
+      - name: Find Comment
+        uses: peter-evans/find-comment@v3
+        id: fc
         with:
-          script: |
-            const comment = [
-               '## FLASH Analysis',
-               '<details>',
-               '<summary>px4_fmu-v5x</summary>',
-               '',
-               '```',
-               `${{ needs.analyze_flash.outputs.px4_fmu-v5x }}`,
-               '```',
-               '</details>',
-               '',
-               '<details>',
-               '<summary>px4_fmu-v6x</summary>',
-               '',
-               '```',
-               `${{ needs.analyze_flash.outputs.px4_fmu-v6x }}`,
-               '```',
-               '</details>'
-            ]
-            github.rest.issues.createComment({
-              issue_number: context.issue.number,
-              owner: context.repo.owner,
-              repo: context.repo.repo,
-              body: comment.join('\n')
-            })
+          issue-number: ${{ github.event.pull_request.number }}
+          comment-author: 'github-actions[bot]'
+          body-includes: FLASH Analysis
+
+      - name: Set Build Time
+        id: bt
+        run: |
+          echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT
+
+      - name: Create or update comment
+        uses: peter-evans/create-or-update-comment@v4
+        with:
+          comment-id: ${{ steps.fc.outputs.comment-id }}
+          issue-number: ${{ github.event.pull_request.number }}
+          body: |
+            ## FLASH Analysis
+            <details>
+              <summary>px4_fmu-v5x</summary>
+
+              ```
+              ${{ needs.analyze_flash.outputs.px4_fmu-v5x }}
+              ```
+            </details>
+
+            <details>
+              <summary>px4_fmu-v6x</summary>
+
+              ```
+              ${{ needs.analyze_flash.outputs.px4_fmu-v6x }}
+              ```
+            </details>
+
+            **Updated: _${{ steps.bt.outputs.timestamp }}_**
+          edit-mode: replace

CMakeLists.txt

@@ -241,19 +241,24 @@ if(NOT CMAKE_BUILD_TYPE)
 endif()
 
 if((CMAKE_BUILD_TYPE STREQUAL "Debug") OR (CMAKE_BUILD_TYPE STREQUAL "Coverage"))
-	set(MAX_CUSTOM_OPT_LEVEL -O0)
+	set(MAX_CUSTOM_OPT_LEVEL_SPEED -O0)
 elseif(CMAKE_BUILD_TYPE MATCHES "Sanitizer")
-	set(MAX_CUSTOM_OPT_LEVEL -O1)
+	set(MAX_CUSTOM_OPT_LEVEL_SPEED -O1)
 elseif(CMAKE_BUILD_TYPE MATCHES "Release")
-	set(MAX_CUSTOM_OPT_LEVEL -O3)
+	set(MAX_CUSTOM_OPT_LEVEL_SPEED -O3)
 else()
 	if(px4_constrained_flash_build)
-		set(MAX_CUSTOM_OPT_LEVEL -Os)
+		set(MAX_CUSTOM_OPT_LEVEL_SPEED -Os)
 	else()
-		set(MAX_CUSTOM_OPT_LEVEL -O2)
+		set(MAX_CUSTOM_OPT_LEVEL_SPEED -O2)
+		set(MAX_CUSTOM_OPT_LEVEL_SPACE -Os)
 	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}")
 

ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter

@@ -44,6 +44,10 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_REV 96 # invert both elevons
 
+# Single-EKF (for replay)
+param set-default EKF2_MULTI_IMU 0
+param set-default SENS_IMU_MODE 1
+
 param set-default FW_P_TC 0.6
 
 param set-default FW_PR_FF 0.0

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -173,6 +173,8 @@ else
 
 	# EKF2 specifics
 	param set-default EKF2_GPS_DELAY 10
+	param set-default EKF2_MULTI_IMU 3
+	param set-default SENS_IMU_MODE 0
 
 	simulator_tcp_port=$((4560+px4_instance))
 

Tools/setup/ubuntu.sh

@@ -64,6 +64,7 @@ sudo apt-get update -y --quiet
 sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 	astyle \
 	build-essential \
+	ccache \
 	cmake \
 	cppcheck \
 	file \

platforms/common/px4_work_queue/CMakeLists.txt

@@ -43,4 +43,4 @@ if(PX4_TESTING)
 	add_subdirectory(test)
 endif()
 
-target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})

platforms/common/uORB/CMakeLists.txt

@@ -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} -D__KERNEL__)
+	target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED} -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})
+target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 if(PX4_TESTING)
 	add_subdirectory(uORB_tests)

platforms/nuttx/src/px4/nxp/imxrt/dshot/CMakeLists.txt

@@ -34,4 +34,4 @@
 px4_add_library(arch_dshot
 	dshot.c
 )
-target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})

platforms/nuttx/src/px4/nxp/imxrt/spi/CMakeLists.txt

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

platforms/nuttx/src/px4/rpi/rpi_common/hrt/CMakeLists.txt

@@ -36,6 +36,6 @@ px4_add_library(arch_hrt
 )
 target_compile_options(arch_hrt
 	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 		-Wno-cast-align # TODO: fix and enable
 )

platforms/nuttx/src/px4/rpi/rpi_common/io_pins/CMakeLists.txt

@@ -39,6 +39,6 @@ px4_add_library(arch_io_pins
 	rp2040_pinset.c
 )
 
-target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 target_link_libraries(arch_io_pins PRIVATE drivers_board)

platforms/nuttx/src/px4/rpi/rpi_common/spi/CMakeLists.txt

@@ -34,4 +34,4 @@
 px4_add_library(arch_spi
 	spi.cpp
 )
-target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})

platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt

@@ -34,4 +34,4 @@
 px4_add_library(arch_dshot
 	dshot.c
 )
-target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})

platforms/nuttx/src/px4/stm/stm32_common/hrt/CMakeLists.txt

@@ -36,6 +36,6 @@ px4_add_library(arch_hrt
 )
 target_compile_options(arch_hrt
 	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 		-Wno-cast-align # TODO: fix and enable
 )

platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt

@@ -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})
+target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 target_link_libraries(arch_io_pins PRIVATE drivers_board) # timer_io_channels

platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt

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

src/drivers/imu/analog_devices/adis16448/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE drivers__imu__analog_devices__adis16448
 	MAIN adis16448
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPACE}
 		#-DDEBUG_BUILD
 	SRCS
 		ADIS16448.cpp

src/drivers/imu/invensense/icm42688p/CMakeLists.txt

@@ -34,7 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__icm42688p
 	MAIN icm42688p
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPACE}
 		#-DDEBUG_BUILD
 	SRCS
 		icm42688p_main.cpp

src/drivers/imu/invensense/icm45686/CMakeLists.txt

@@ -34,7 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__icm45686
 	MAIN icm45686
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPACE}
 		#-DDEBUG_BUILD
 	SRCS
 		icm45686_main.cpp

src/drivers/imu/invensense/iim42652/CMakeLists.txt

@@ -34,7 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__iim42652
 	MAIN iim42652
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPACE}
 		#-DDEBUG_BUILD
 	SRCS
 		iim42652_main.cpp

src/drivers/imu/invensense/iim42653/CMakeLists.txt

@@ -34,7 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__iim42653
 	MAIN iim42653
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPACE}
 		#-DDEBUG_BUILD
 	SRCS
 		iim42653_main.cpp

src/examples/fake_imu/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__fake_imu
 	MAIN fake_imu
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPACE}
 	SRCS
 		FakeImu.cpp
 		FakeImu.hpp

src/lib/cdev/CMakeLists.txt

@@ -49,7 +49,7 @@ px4_add_library(cdev
 	CDev.hpp
 	${SRCS_PLATFORM}
 )
-target_compile_options(cdev PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(cdev PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 if (${PX4_PLATFORM} STREQUAL "nuttx")
 	target_link_libraries(cdev PRIVATE nuttx_fs) # register/unregister driver

src/lib/conversion/CMakeLists.txt

@@ -36,6 +36,6 @@ px4_add_library(conversion
 	rotation.h
 )
 
-target_compile_options(conversion PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(conversion PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 px4_add_unit_gtest(SRC RotationTest.cpp LINKLIBS conversion)

src/lib/crc/CMakeLists.txt

@@ -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})
+target_compile_options(crc PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})

src/lib/drivers/accelerometer/CMakeLists.txt

@@ -35,5 +35,5 @@ px4_add_library(drivers_accelerometer
 	PX4Accelerometer.cpp
 	PX4Accelerometer.hpp
 )
-target_compile_options(drivers_accelerometer PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(drivers_accelerometer PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPACE})
 target_link_libraries(drivers_accelerometer PRIVATE conversion)

src/lib/drivers/gyroscope/CMakeLists.txt

@@ -35,5 +35,5 @@ px4_add_library(drivers_gyroscope
 	PX4Gyroscope.cpp
 	PX4Gyroscope.hpp
 )
-target_compile_options(drivers_gyroscope PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(drivers_gyroscope PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPACE})
 target_link_libraries(drivers_gyroscope PRIVATE conversion)

src/lib/geo/CMakeLists.txt

@@ -36,6 +36,6 @@ add_library(geo
 	geo.h
 )
 add_dependencies(geo prebuild_targets)
-target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 px4_add_unit_gtest(SRC test_geo.cpp LINKLIBS geo)

src/lib/heatshrink/CMakeLists.txt

@@ -38,5 +38,5 @@ px4_add_library(heatshrink
 )
 
 target_compile_options(heatshrink PRIVATE
-	${MAX_CUSTOM_OPT_LEVEL}
+	${MAX_CUSTOM_OPT_LEVEL_SPEED}
 	-DHEATSHRINK_DYNAMIC_ALLOC=0)

src/lib/mixer_module/CMakeLists.txt

@@ -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})
+target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 target_include_directories(mixer_module PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
 
 px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module)

src/lib/parameters/CMakeLists.txt

@@ -31,7 +31,7 @@
 #
 ############################################################################
 
-add_compile_options(${MAX_CUSTOM_OPT_LEVEL})
+add_compile_options(${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 if (NOT PARAM_DEFAULT_OVERRIDES)
 	set(PARAM_DEFAULT_OVERRIDES "{}")
@@ -157,7 +157,7 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp)
 set(SRCS)
 
 list(APPEND SRCS
-	parameters.cpp 
+	parameters.cpp
 	atomic_transaction.cpp
 	autosave.cpp
 )

src/lib/perf/CMakeLists.txt

@@ -33,4 +33,4 @@
 
 add_library(perf perf_counter.cpp)
 add_dependencies(perf prebuild_targets)
-target_compile_options(perf PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(perf PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})

src/lib/pid/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,4 +31,10 @@
 #
 ############################################################################
 
-px4_add_library(pid pid.cpp)
+px4_add_library(PID
+	PID.cpp
+	PID.hpp
+)
+target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)

src/lib/pid/PID.cpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "PID.hpp"
+#include "lib/mathlib/math/Functions.hpp"
+
+void PID::setGains(const float P, const float I, const float D)
+{
+	_gain_proportional = P;
+	_gain_integral = I;
+	_gain_derivative = D;
+}
+
+float PID::update(const float feedback, const float dt, const bool update_integral)
+{
+	const float error = _setpoint - feedback;
+	const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt));
+
+	if (update_integral) {
+		updateIntegral(error, dt);
+	}
+
+	_last_feedback = feedback;
+	return math::constrain(output, -_limit_output, _limit_output);
+}
+
+void PID::updateIntegral(float error, const float dt)
+{
+	const float integral_new = _integral + _gain_integral * error * dt;
+
+	if (std::isfinite(integral_new)) {
+		_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
+	}
+}
+
+float PID::updateDerivative(float feedback, const float dt)
+{
+	float feedback_change = 0.f;
+
+	if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) {
+		feedback_change = (feedback - _last_feedback) / dt;
+	}
+
+	return feedback_change;
+}

src/lib/pid/PID.hpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+class PID
+{
+public:
+	PID() = default;
+	virtual ~PID() = default;
+	void setOutputLimit(const float limit) { _limit_output = limit; }
+	void setIntegralLimit(const float limit) { _limit_integral = limit; }
+	void setGains(const float P, const float I, const float D);
+	void setSetpoint(const float setpoint) { _setpoint = setpoint; }
+	float update(const float feedback, const float dt, const bool update_integral = true);
+	float getIntegral() { return _integral; }
+	void resetIntegral() { _integral = 0.f; };
+	void resetDerivative() { _last_feedback = NAN; };
+private:
+	void updateIntegral(float error, const float dt);
+	float updateDerivative(float feedback, const float dt);
+
+	float _setpoint{0.f}; ///< current setpoint to track
+	float _integral{0.f}; ///< integral state
+	float _last_feedback{NAN};
+
+	// Gains, Limits
+	float _gain_proportional{0.f};
+	float _gain_integral{0.f};
+	float _gain_derivative{0.f};
+	float _limit_integral{0.f};
+	float _limit_output{0.f};
+};

src/lib/pid/PIDTest.cpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <PID.hpp>
+
+TEST(PIDTest, AllZeroCase)
+{
+	PID pid;
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+}
+
+TEST(PIDTest, OutputLimit)
+{
+	PID pid;
+	pid.setOutputLimit(.01f);
+	pid.setGains(.1f, 0.f, 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
+	EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
+	EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+	EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
+}
+
+TEST(PIDTest, ProportinalOnly)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(.1f, 0.f, 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+
+	float plant = 0.f;
+	float output = 10000.f;
+	int i; // need function scope to check how many steps
+
+	for (i = 1000; i > 0; i--) {
+		const float output_new = pid.update(plant, 0.f, false);
+		plant += output_new;
+
+		// expect the output to get smaller with each iteration
+		if (output_new >= output) {
+			break;
+		}
+
+		output = output_new;
+	}
+
+	EXPECT_FLOAT_EQ(plant, 1.f);
+	EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
+}
+
+TEST(PIDTest, InteralOpenLoop)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(0.f, .1f, 0.f);
+	pid.setIntegralLimit(.05f);
+	pid.setSetpoint(1.f);
+
+	// Zero error
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+
+	// Open loop ramp up
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+
+	// Open loop ramp down
+	pid.setSetpoint(-1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	pid.resetIntegral();
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
+}

src/lib/pid/pid.cpp

@@ -1,185 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file pid.cpp
- *
- * Implementation of generic PID controller.
- *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#include "pid.h"
-#include <math.h>
-#include <px4_platform_common/defines.h>
-
-#define SIGMA 0.000001f
-
-__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
-{
-	pid->mode = mode;
-	pid->dt_min = dt_min;
-	pid->kp = 0.0f;
-	pid->ki = 0.0f;
-	pid->kd = 0.0f;
-	pid->integral = 0.0f;
-	pid->integral_limit = 0.0f;
-	pid->output_limit = 0.0f;
-	pid->error_previous = 0.0f;
-	pid->last_output = 0.0f;
-}
-
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
-{
-	int ret = 0;
-
-	if (PX4_ISFINITE(kp)) {
-		pid->kp = kp;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(ki)) {
-		pid->ki = ki;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(kd)) {
-		pid->kd = kd;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(integral_limit)) {
-		pid->integral_limit = integral_limit;
-
-	}  else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(output_limit)) {
-		pid->output_limit = output_limit;
-
-	}  else {
-		ret = 1;
-	}
-
-	return ret;
-}
-
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
-{
-	if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
-		return pid->last_output;
-	}
-
-	float i, d;
-
-	/* current error value */
-	float error = sp - val;
-
-	/* current error derivative */
-	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = error;
-
-	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
-		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = -val;
-
-	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
-		d = -val_dot;
-
-	} else {
-		d = 0.0f;
-	}
-
-	if (!PX4_ISFINITE(d)) {
-		d = 0.0f;
-	}
-
-	/* calculate PD output */
-	float output = (error * pid->kp) + (d * pid->kd);
-
-	if (pid->ki > SIGMA) {
-		// Calculate the error integral and check for saturation
-		i = pid->integral + (error * dt);
-
-		/* check for saturation */
-		if (PX4_ISFINITE(i)) {
-			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
-			    fabsf(i) <= pid->integral_limit) {
-				/* not saturated, use new integral value */
-				pid->integral = i;
-			}
-		}
-
-		/* add I component to output */
-		output += pid->integral * pid->ki;
-	}
-
-	/* limit output */
-	if (PX4_ISFINITE(output)) {
-		if (pid->output_limit > SIGMA) {
-			if (output > pid->output_limit) {
-				output = pid->output_limit;
-
-			} else if (output < -pid->output_limit) {
-				output = -pid->output_limit;
-			}
-		}
-
-		pid->last_output = output;
-	}
-
-	return pid->last_output;
-}
-
-
-__EXPORT void pid_reset_integral(PID_t *pid)
-{
-	pid->integral = 0.0f;
-}

src/lib/pid/pid.h

@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file pid.h
- *
- * Definition of generic PID controller.
- *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#ifndef PID_H_
-#define PID_H_
-
-#include <stdint.h>
-
-__BEGIN_DECLS
-
-typedef enum PID_MODE {
-	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
-	PID_MODE_DERIVATIV_NONE = 0,
-	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
-	 * val_dot in pid_calculate() will be ignored */
-	PID_MODE_DERIVATIV_CALC,
-	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
-	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
-	PID_MODE_DERIVATIV_CALC_NO_SP,
-	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-	PID_MODE_DERIVATIV_SET
-} pid_mode_t;
-
-typedef struct {
-	pid_mode_t mode;
-	float dt_min;
-	float kp;
-	float ki;
-	float kd;
-	float integral;
-	float integral_limit;
-	float output_limit;
-	float error_previous;
-	float last_output;
-} PID_t;
-
-__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
-__EXPORT void pid_reset_integral(PID_t *pid);
-
-__END_DECLS
-
-#endif /* PID_H_ */

src/lib/rate_control/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_library(RateControl
 	rate_control.cpp
 	rate_control.hpp
 )
-target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(RateControl PRIVATE mathlib)
 

src/lib/systemlib/CMakeLists.txt

@@ -47,4 +47,4 @@ if(${PX4_PLATFORM} STREQUAL "nuttx")
 endif()
 
 px4_add_library(systemlib ${SRCS})
-target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})

src/lib/tinybson/CMakeLists.txt

@@ -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}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 )
 
 add_dependencies(tinybson prebuild_targets)

src/lib/world_magnetic_model/CMakeLists.txt

@@ -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})
+target_compile_options(world_magnetic_model PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 if(BUILD_TESTING)
 	px4_add_unit_gtest(SRC test_geo_lookup.cpp LINKLIBS world_magnetic_model)

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -64,7 +64,7 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessRoverAckermann.cpp
 )
 
-target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE

src/modules/control_allocator/CMakeLists.txt

@@ -39,7 +39,7 @@ px4_add_module(
 	MODULE modules__control_allocator
 	MAIN control_allocator
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 	STACK_MAIN
 		3000
 	SRCS

src/modules/control_allocator/ControlAllocation/CMakeLists.txt

@@ -39,7 +39,7 @@ px4_add_library(ControlAllocation
 	ControlAllocationSequentialDesaturation.cpp
 	ControlAllocationSequentialDesaturation.hpp
 )
-target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(ControlAllocation PRIVATE mathlib)
 

src/modules/ekf2/CMakeLists.txt

@@ -238,7 +238,7 @@ px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 		-fno-associative-math
 		#-DDEBUG_BUILD
 		#-O0

src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt

@@ -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})
+target_compile_options(lat_lon_alt PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS lat_lon_alt geo)

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

@@ -337,9 +337,7 @@ def predict_sideslip(
     vel_rel = state["vel"] - wind
     relative_wind_body = state["quat_nominal"].inverse() * vel_rel
 
-    # Small angle approximation of side slip model
-    # Protect division by zero using epsilon
-    sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon)
+    sideslip_pred = sf.atan2(relative_wind_body[1], relative_wind_body[0], epsilon)
 
     return sideslip_pred
 

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

@@ -30,66 +30,65 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
                           const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
                           const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
                           matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 513
+  // Total ops: 518
 
   // Input arrays
 
-  // Intermediate terms (43)
-  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
-  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp3 = 4 * _tmp2;
-  const Scalar _tmp4 = 2 * state(6, 0);
-  const Scalar _tmp5 = _tmp4 * state(0, 0);
-  const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp8 = 2 * state(0, 0);
-  const Scalar _tmp9 = _tmp8 * state(3, 0);
-  const Scalar _tmp10 = 2 * state(2, 0);
-  const Scalar _tmp11 = _tmp10 * state(1, 0);
-  const Scalar _tmp12 = _tmp11 + _tmp9;
-  const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0);
-  const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7;
-  const Scalar _tmp15 =
-      _tmp14 + epsilon * (2 * math::min<Scalar>(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1);
-  const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp17 = _tmp11 - _tmp9;
-  const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0);
-  const Scalar _tmp19 =
-      (_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2));
-  const Scalar _tmp20 = _tmp4 * state(3, 0);
-  const Scalar _tmp21 = Scalar(1.0) / (_tmp15);
-  const Scalar _tmp22 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20);
-  const Scalar _tmp23 = 4 * _tmp0;
-  const Scalar _tmp24 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5);
-  const Scalar _tmp25 = 2 * state(3, 0);
-  const Scalar _tmp26 = _tmp4 * state(2, 0);
-  const Scalar _tmp27 = _tmp4 * state(1, 0);
-  const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) +
-                        (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27);
-  const Scalar _tmp29 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26);
+  // Intermediate terms (50)
+  const Scalar _tmp0 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp1 = 2 * _tmp0;
+  const Scalar _tmp2 = 2 * state(3, 0);
+  const Scalar _tmp3 = _tmp2 * state(6, 0);
+  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp6 = _tmp2 * state(0, 0);
+  const Scalar _tmp7 = 2 * state(1, 0);
+  const Scalar _tmp8 = _tmp7 * state(2, 0);
+  const Scalar _tmp9 = _tmp6 + _tmp8;
+  const Scalar _tmp10 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp11 = 2 * state(2, 0);
+  const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0);
+  const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0);
+  const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
+  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
+  const Scalar _tmp16 = 2 * _tmp10;
+  const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0);
+  const Scalar _tmp18 = std::pow(_tmp14, Scalar(2));
+  const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp20 = -_tmp6 + _tmp8;
+  const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0);
+  const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0);
+  const Scalar _tmp23 = _tmp22 / _tmp18;
+  const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) -
+                        _tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17);
+  const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2)));
+  const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25;
+  const Scalar _tmp27 = _tmp26 * state(3, 0);
+  const Scalar _tmp28 = _tmp7 * state(6, 0);
+  const Scalar _tmp29 = _tmp11 * state(6, 0);
   const Scalar _tmp30 =
-      -_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0);
-  const Scalar _tmp31 =
-      _tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0);
-  const Scalar _tmp32 =
-      _tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0);
-  const Scalar _tmp33 = _tmp19 * _tmp7;
-  const Scalar _tmp34 = _tmp17 * _tmp21;
-  const Scalar _tmp35 = -_tmp33 + _tmp34;
-  const Scalar _tmp36 = _tmp12 * _tmp19;
-  const Scalar _tmp37 = _tmp16 * _tmp21;
-  const Scalar _tmp38 = -_tmp36 + _tmp37;
-  const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21;
-  const Scalar _tmp40 = _tmp33 - _tmp34;
-  const Scalar _tmp41 = _tmp36 - _tmp37;
-  const Scalar _tmp42 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
+      _tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29);
+  const Scalar _tmp31 = _tmp26 * state(1, 0);
+  const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) -
+                        _tmp23 * (_tmp16 * state(2, 0) + _tmp3);
+  const Scalar _tmp33 = _tmp26 * state(0, 0);
+  const Scalar _tmp34 = 4 * state(3, 0);
+  const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) -
+                        _tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28);
+  const Scalar _tmp36 = _tmp26 * state(2, 0);
+  const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36;
+  const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35;
+  const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35;
+  const Scalar _tmp40 = _tmp23 * _tmp5;
+  const Scalar _tmp41 = _tmp15 * _tmp20;
+  const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41);
+  const Scalar _tmp43 = _tmp15 * _tmp19;
+  const Scalar _tmp44 = _tmp23 * _tmp9;
+  const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44);
+  const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21);
+  const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41);
+  const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44);
+  const Scalar _tmp49 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
 
   // Output terms (2)
   if (H != nullptr) {
@@ -97,91 +96,91 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
 
     _h.setZero();
 
-    _h(0, 0) = _tmp30;
-    _h(1, 0) = _tmp31;
-    _h(2, 0) = _tmp32;
-    _h(3, 0) = _tmp35;
-    _h(4, 0) = _tmp38;
-    _h(5, 0) = _tmp39;
-    _h(21, 0) = _tmp40;
-    _h(22, 0) = _tmp41;
+    _h(0, 0) = _tmp37;
+    _h(1, 0) = _tmp38;
+    _h(2, 0) = _tmp39;
+    _h(3, 0) = _tmp42;
+    _h(4, 0) = _tmp45;
+    _h(5, 0) = _tmp46;
+    _h(21, 0) = _tmp47;
+    _h(22, 0) = _tmp48;
   }
 
   if (K != nullptr) {
     matrix::Matrix<Scalar, 24, 1>& _k = (*K);
 
     _k(0, 0) =
-        _tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 +
-                  P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39);
+        _tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 +
+                  P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46);
     _k(1, 0) =
-        _tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 +
-                  P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39);
+        _tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 +
+                  P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46);
     _k(2, 0) =
-        _tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 +
-                  P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39);
+        _tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 +
+                  P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46);
     _k(3, 0) =
-        _tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 +
-                  P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39);
+        _tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 +
+                  P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46);
     _k(4, 0) =
-        _tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 +
-                  P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39);
+        _tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 +
+                  P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46);
     _k(5, 0) =
-        _tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 +
-                  P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39);
+        _tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 +
+                  P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46);
     _k(6, 0) =
-        _tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 +
-                  P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39);
+        _tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 +
+                  P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46);
     _k(7, 0) =
-        _tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 +
-                  P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39);
+        _tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 +
+                  P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46);
     _k(8, 0) =
-        _tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 +
-                  P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39);
+        _tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 +
+                  P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46);
     _k(9, 0) =
-        _tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 +
-                  P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39);
+        _tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 +
+                  P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46);
     _k(10, 0) =
-        _tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 +
-                  P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39);
+        _tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 +
+                  P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46);
     _k(11, 0) =
-        _tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 +
-                  P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39);
+        _tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 +
+                  P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46);
     _k(12, 0) =
-        _tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 +
-                  P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39);
+        _tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 +
+                  P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46);
     _k(13, 0) =
-        _tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 +
-                  P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39);
+        _tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 +
+                  P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46);
     _k(14, 0) =
-        _tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 +
-                  P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39);
+        _tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 +
+                  P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46);
     _k(15, 0) =
-        _tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 +
-                  P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39);
+        _tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 +
+                  P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46);
     _k(16, 0) =
-        _tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 +
-                  P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39);
+        _tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 +
+                  P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46);
     _k(17, 0) =
-        _tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 +
-                  P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39);
+        _tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 +
+                  P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46);
     _k(18, 0) =
-        _tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 +
-                  P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39);
+        _tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 +
+                  P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46);
     _k(19, 0) =
-        _tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 +
-                  P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39);
+        _tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 +
+                  P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46);
     _k(20, 0) =
-        _tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 +
-                  P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39);
+        _tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 +
+                  P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46);
     _k(21, 0) =
-        _tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 +
-                  P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39);
+        _tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 +
+                  P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46);
     _k(22, 0) =
-        _tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 +
-                  P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39);
+        _tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 +
+                  P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46);
     _k(23, 0) =
-        _tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 +
-                  P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39);
+        _tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 +
+                  P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46);
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -30,70 +30,69 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
                                      const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                      const Scalar epsilon, Scalar* const innov = nullptr,
                                      Scalar* const innov_var = nullptr) {
-  // Total ops: 265
+  // Total ops: 266
 
   // Input arrays
 
-  // Intermediate terms (42)
+  // Intermediate terms (49)
   const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp3 = 2 * state(0, 0);
-  const Scalar _tmp4 = _tmp3 * state(3, 0);
+  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp2 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp3 = 2 * state(3, 0);
+  const Scalar _tmp4 = _tmp3 * state(0, 0);
   const Scalar _tmp5 = 2 * state(1, 0);
   const Scalar _tmp6 = _tmp5 * state(2, 0);
-  const Scalar _tmp7 = _tmp4 + _tmp6;
-  const Scalar _tmp8 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp7 = -_tmp4 + _tmp6;
+  const Scalar _tmp8 = -state(22, 0) + state(4, 0);
   const Scalar _tmp9 = 2 * state(2, 0);
-  const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp10 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
   const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
-  const Scalar _tmp12 =
-      _tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
-  const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
-  const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp15 = -_tmp4 + _tmp6;
-  const Scalar _tmp16 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
-  const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
-  const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
-  const Scalar _tmp19 = _tmp18 * _tmp7;
-  const Scalar _tmp20 = _tmp13 * _tmp14;
-  const Scalar _tmp21 = _tmp19 - _tmp20;
-  const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
-  const Scalar _tmp23 = _tmp1 * _tmp18;
-  const Scalar _tmp24 = _tmp13 * _tmp15;
-  const Scalar _tmp25 = -_tmp23 + _tmp24;
-  const Scalar _tmp26 = 2 * state(6, 0);
-  const Scalar _tmp27 = _tmp26 * state(0, 0);
-  const Scalar _tmp28 = _tmp26 * state(3, 0);
-  const Scalar _tmp29 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) -
-      Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8);
-  const Scalar _tmp30 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) -
-      Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9);
-  const Scalar _tmp31 = 2 * state(3, 0);
-  const Scalar _tmp32 = _tmp26 * state(2, 0);
-  const Scalar _tmp33 = _tmp26 * state(1, 0);
-  const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) -
-                        Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32);
-  const Scalar _tmp35 = 4 * state(3, 0);
-  const Scalar _tmp36 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) -
-      Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33);
-  const Scalar _tmp37 =
-      _tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0);
-  const Scalar _tmp38 =
-      -_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0);
-  const Scalar _tmp39 =
-      _tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0);
-  const Scalar _tmp40 = _tmp23 - _tmp24;
-  const Scalar _tmp41 = -_tmp19 + _tmp20;
+  const Scalar _tmp12 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp13 = _tmp4 + _tmp6;
+  const Scalar _tmp14 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp15 = _tmp12 * _tmp8 + _tmp13 * _tmp2 + _tmp14 * state(6, 0);
+  const Scalar _tmp16 = _tmp15 + epsilon * ((((_tmp15) > 0) - ((_tmp15) < 0)) + Scalar(0.5));
+  const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
+  const Scalar _tmp18 = _tmp1 * _tmp17;
+  const Scalar _tmp19 = std::pow(_tmp16, Scalar(2));
+  const Scalar _tmp20 = _tmp11 / _tmp19;
+  const Scalar _tmp21 = _tmp13 * _tmp20;
+  const Scalar _tmp22 = _tmp19 / (std::pow(_tmp11, Scalar(2)) + _tmp19);
+  const Scalar _tmp23 = _tmp22 * (-_tmp18 + _tmp21);
+  const Scalar _tmp24 = _tmp12 * _tmp20;
+  const Scalar _tmp25 = _tmp17 * _tmp7;
+  const Scalar _tmp26 = _tmp22 * (-_tmp24 + _tmp25);
+  const Scalar _tmp27 = _tmp22 * (_tmp10 * _tmp17 - _tmp14 * _tmp20);
+  const Scalar _tmp28 = _tmp3 * state(6, 0);
+  const Scalar _tmp29 = 4 * _tmp8;
+  const Scalar _tmp30 = 2 * state(0, 0);
+  const Scalar _tmp31 = _tmp30 * state(6, 0);
+  const Scalar _tmp32 =
+      _tmp17 * (_tmp28 + _tmp5 * _tmp8) - _tmp20 * (_tmp2 * _tmp5 - _tmp29 * state(2, 0) - _tmp31);
+  const Scalar _tmp33 = (Scalar(1) / Scalar(2)) * _tmp22;
+  const Scalar _tmp34 = _tmp33 * state(1, 0);
+  const Scalar _tmp35 = _tmp5 * state(6, 0);
+  const Scalar _tmp36 = _tmp9 * state(6, 0);
+  const Scalar _tmp37 = _tmp17 * (-_tmp3 * _tmp8 + _tmp35) - _tmp20 * (_tmp2 * _tmp3 - _tmp36);
+  const Scalar _tmp38 = _tmp33 * state(3, 0);
+  const Scalar _tmp39 = 4 * _tmp2;
+  const Scalar _tmp40 =
+      _tmp17 * (_tmp31 - _tmp39 * state(1, 0) + _tmp8 * _tmp9) - _tmp20 * (_tmp2 * _tmp9 + _tmp28);
+  const Scalar _tmp41 = _tmp33 * state(2, 0);
+  const Scalar _tmp42 = _tmp17 * (-_tmp30 * _tmp8 + _tmp36 - _tmp39 * state(3, 0)) -
+                        _tmp20 * (_tmp2 * _tmp30 - _tmp29 * state(3, 0) + _tmp35);
+  const Scalar _tmp43 = _tmp33 * state(0, 0);
+  const Scalar _tmp44 = _tmp32 * _tmp34 - _tmp37 * _tmp38 - _tmp40 * _tmp41 + _tmp42 * _tmp43;
+  const Scalar _tmp45 = -_tmp32 * _tmp38 - _tmp34 * _tmp37 + _tmp40 * _tmp43 + _tmp41 * _tmp42;
+  const Scalar _tmp46 = _tmp32 * _tmp43 - _tmp34 * _tmp42 - _tmp37 * _tmp41 + _tmp38 * _tmp40;
+  const Scalar _tmp47 = _tmp22 * (_tmp24 - _tmp25);
+  const Scalar _tmp48 = _tmp22 * (_tmp18 - _tmp21);
 
   // Output terms (2)
   if (innov != nullptr) {
     Scalar& _innov = (*innov);
 
-    _innov = _tmp13 * _tmp17;
+    _innov = std::atan2(_tmp11, _tmp16);
   }
 
   if (innov_var != nullptr) {
@@ -101,22 +100,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
 
     _innov_var =
         R +
-        _tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 +
-                  P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) +
-        _tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 +
-                  P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) +
-        _tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 +
-                  P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) +
-        _tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 +
-                  P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) +
-        _tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 +
-                  P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) +
-        _tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 +
-                  P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) +
-        _tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 +
-                  P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) +
-        _tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 +
-                  P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22);
+        _tmp23 * (P(0, 22) * _tmp45 + P(1, 22) * _tmp46 + P(2, 22) * _tmp44 + P(21, 22) * _tmp47 +
+                  P(22, 22) * _tmp23 + P(3, 22) * _tmp26 + P(4, 22) * _tmp48 + P(5, 22) * _tmp27) +
+        _tmp26 * (P(0, 3) * _tmp45 + P(1, 3) * _tmp46 + P(2, 3) * _tmp44 + P(21, 3) * _tmp47 +
+                  P(22, 3) * _tmp23 + P(3, 3) * _tmp26 + P(4, 3) * _tmp48 + P(5, 3) * _tmp27) +
+        _tmp27 * (P(0, 5) * _tmp45 + P(1, 5) * _tmp46 + P(2, 5) * _tmp44 + P(21, 5) * _tmp47 +
+                  P(22, 5) * _tmp23 + P(3, 5) * _tmp26 + P(4, 5) * _tmp48 + P(5, 5) * _tmp27) +
+        _tmp44 * (P(0, 2) * _tmp45 + P(1, 2) * _tmp46 + P(2, 2) * _tmp44 + P(21, 2) * _tmp47 +
+                  P(22, 2) * _tmp23 + P(3, 2) * _tmp26 + P(4, 2) * _tmp48 + P(5, 2) * _tmp27) +
+        _tmp45 * (P(0, 0) * _tmp45 + P(1, 0) * _tmp46 + P(2, 0) * _tmp44 + P(21, 0) * _tmp47 +
+                  P(22, 0) * _tmp23 + P(3, 0) * _tmp26 + P(4, 0) * _tmp48 + P(5, 0) * _tmp27) +
+        _tmp46 * (P(0, 1) * _tmp45 + P(1, 1) * _tmp46 + P(2, 1) * _tmp44 + P(21, 1) * _tmp47 +
+                  P(22, 1) * _tmp23 + P(3, 1) * _tmp26 + P(4, 1) * _tmp48 + P(5, 1) * _tmp27) +
+        _tmp47 * (P(0, 21) * _tmp45 + P(1, 21) * _tmp46 + P(2, 21) * _tmp44 + P(21, 21) * _tmp47 +
+                  P(22, 21) * _tmp23 + P(3, 21) * _tmp26 + P(4, 21) * _tmp48 + P(5, 21) * _tmp27) +
+        _tmp48 * (P(0, 4) * _tmp45 + P(1, 4) * _tmp46 + P(2, 4) * _tmp44 + P(21, 4) * _tmp47 +
+                  P(22, 4) * _tmp23 + P(3, 4) * _tmp26 + P(4, 4) * _tmp48 + P(5, 4) * _tmp27);
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_library(follow_target_estimator
 	TargetEstimator.cpp
 )
 
-target_compile_options(follow_target_estimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(follow_target_estimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 
 target_link_libraries(follow_target_estimator
 	PRIVATE

src/modules/fw_autotune_attitude_control/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE fw_autotune_attitude_control
 	MAIN fw_autotune_attitude_control
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 	SRCS
 		fw_autotune_attitude_control.cpp
 		fw_autotune_attitude_control.hpp

src/modules/gyro_fft/CMakeLists.txt

@@ -51,7 +51,7 @@ px4_add_module(
 	STACK_MAIN
 		4096
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 		-DARM_ALL_FFT_TABLES
 		-DARM_MATH_LOOPUNROLL
 	INCLUDES

src/modules/logger/CMakeLists.txt

@@ -37,7 +37,7 @@ px4_add_module(
 	PRIORITY "SCHED_PRIORITY_MAX-30"
 	STACK_MAIN 2500
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 		-Wno-cast-align # TODO: fix and enable
 	SRCS
 		logged_topics.cpp

src/modules/mc_att_control/AttitudeControl/CMakeLists.txt

@@ -36,7 +36,7 @@ px4_add_library(AttitudeControl
 	AttitudeControl.hpp
 	AttitudeControlMath.hpp
 )
-target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
 target_include_directories(AttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl)

src/modules/mc_att_control/CMakeLists.txt

@@ -37,7 +37,7 @@ px4_add_module(
 	MODULE modules__mc_att_control
 	MAIN mc_att_control
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 	SRCS
 		mc_att_control_main.cpp
 		mc_att_control.hpp

src/modules/mc_autotune_attitude_control/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE mc_autotune_attitude_control
 	MAIN mc_autotune_attitude_control
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 	SRCS
 		mc_autotune_attitude_control.cpp
 		mc_autotune_attitude_control.hpp

src/modules/mc_hover_thrust_estimator/CMakeLists.txt

@@ -40,7 +40,7 @@ px4_add_module(
 	MODULE modules__mc_hover_thrust_estimator
 	MAIN mc_hover_thrust_estimator
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 	SRCS
 		MulticopterHoverThrustEstimator.cpp
 		MulticopterHoverThrustEstimator.hpp

src/modules/mc_rate_control/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__mc_rate_control
 	MAIN mc_rate_control
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 	SRCS
 		MulticopterRateControl.cpp
 		MulticopterRateControl.hpp

src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt

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

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp

@@ -41,27 +41,19 @@ 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_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_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_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
+	_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);
 
 	// Update slew rates
 	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
@@ -117,8 +109,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)
-				steering_setpoint += pid_calculate(&_pid_lat_accel, _rover_ackermann_setpoint.lateral_acceleration_setpoint,
-								   vehicle_lateral_acceleration, 0, dt);
+				_pid_lat_accel.setSetpoint(_rover_ackermann_setpoint.lateral_acceleration_setpoint);
+				steering_setpoint += _pid_lat_accel.update(vehicle_lateral_acceleration, dt);
 			}
 
 			_rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(),
@@ -156,8 +148,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.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.pid_throttle_integral = _pid_throttle.getIntegral();
+	_rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.getIntegral();
 	_rover_ackermann_status_pub.publish(_rover_ackermann_status);
 
 	// Publish to motor
@@ -218,8 +210,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe
 						   -1.f, 1.f);
 		}
 
-		forward_speed_normalized += pid_calculate(&_pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
-					    vehicle_forward_speed, 0, dt); // Feedback
+		_pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState());
+		forward_speed_normalized += _pid_throttle.update(vehicle_forward_speed, dt);
 	}
 
 	return math::constrain(forward_speed_normalized, -1.f, 1.f);
@@ -228,8 +220,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe
 
 void RoverAckermannControl::resetControllers()
 {
-	pid_reset_integral(&_pid_throttle);
-	pid_reset_integral(&_pid_lat_accel);
+	_pid_throttle.resetIntegral();
+	_pid_lat_accel.resetIntegral();
 	_forward_speed_setpoint_with_accel_limit.setForcedValue(0.f);
 	_steering_with_rate_limit.setForcedValue(0.f);
 

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp

@@ -46,7 +46,7 @@
 #include <uORB/topics/actuator_servos.h>
 
 // Standard libraries
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #include <matrix/matrix/math.hpp>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -114,8 +114,8 @@ private:
 	hrt_abstime _timestamp{0};
 
 	// Controllers
-	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
+	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
 	SlewRate<float> _steering_with_rate_limit{0.f};
 	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
 

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

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

src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt

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

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -42,9 +42,6 @@ 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()
@@ -54,24 +51,15 @@ void RoverDifferentialControl::updateParams()
 	_max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F;
 
 	// Update PID
-	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
+	_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);
 
 	// Update slew rates
 	if (_max_yaw_rate > FLT_EPSILON) {
@@ -99,8 +87,10 @@ 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());
-		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);
+		_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);
 		_rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint;
 
 	} else {
@@ -142,9 +132,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.integral;
-	_rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
-	_rover_differential_status.pid_yaw_integral = _pid_yaw.integral;
+	_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_pub.publish(_rover_differential_status);
 
 	// Publish to motors
@@ -158,7 +148,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_t &pid_yaw_rate, const bool normalized)
+		PID &pid_yaw_rate, const bool normalized)
 {
 	float slew_rate_normalization{1.f};
 
@@ -190,8 +180,8 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set
 						max_thr_yaw_r, -1.f, 1.f);
 		}
 
-		speed_diff_normalized += pid_calculate(&pid_yaw_rate, yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0,
-						       dt); // Feedback
+		pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState());
+		speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt);
 	}
 
 	return math::constrain(speed_diff_normalized, -1.f, 1.f);
@@ -200,7 +190,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_t &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized)
+		PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized)
 {
 	float slew_rate_normalization{1.f};
 
@@ -242,8 +232,8 @@ float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_
 						   -1.f, 1.f);
 		}
 
-		forward_speed_normalized += pid_calculate(&pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
-					    vehicle_forward_speed, 0, dt); // Feedback
+		pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState());
+		forward_speed_normalized += pid_throttle.update(vehicle_forward_speed, dt);
 	}
 
 	return math::constrain(forward_speed_normalized, -1.f, 1.f);
@@ -267,7 +257,7 @@ matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forwar
 
 void RoverDifferentialControl::resetControllers()
 {
-	pid_reset_integral(&_pid_throttle);
-	pid_reset_integral(&_pid_yaw_rate);
-	pid_reset_integral(&_pid_yaw);
+	_pid_throttle.resetIntegral();
+	_pid_yaw_rate.resetIntegral();
+	_pid_yaw.resetIntegral();
 }

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp

@@ -47,7 +47,7 @@
 #include <lib/slew_rate/SlewRateYaw.hpp>
 
 // Standard libraries
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #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_t &pid_yaw_rate, bool normalized);
+				      float wheel_track, float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID &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_t &pid_throttle, float max_accel, float max_decel,
+					  SlewRate<float> &forward_speed_setpoint_with_accel_limit, PID &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_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
+	PID _pid_throttle; // Closed loop speed control
+	PID _pid_yaw;      // Closed loop yaw control
+	PID _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;

src/modules/rover_mecanum/RoverMecanum.hpp

@@ -53,7 +53,7 @@
 #include <uORB/topics/rover_mecanum_setpoint.h>
 
 // Standard libraries
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #include <matrix/matrix/math.hpp>
 
 // Local includes

src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt

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

src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp

@@ -42,40 +42,28 @@ 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_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
+	_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);
 }
 
 void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
@@ -91,11 +79,12 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 
 	// Closed loop yaw control
 	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) {
-		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);
+		_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);
 
 	} else {
-		pid_reset_integral(&_pid_yaw);
+		_pid_yaw.resetIntegral();
 	}
 
 	// Yaw rate control
@@ -108,8 +97,9 @@ 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_calculate(&_pid_yaw_rate, _rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0.f, dt),
+							_pid_yaw_rate.update(vehicle_yaw_rate, dt),
 							-1.f, 1.f); // Feedback
 
 	} else { // Use normalized setpoint
@@ -129,8 +119,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);
 		}
 
-		forward_throttle += pid_calculate(&_pid_forward_throttle, _rover_mecanum_setpoint.forward_speed_setpoint,
-						  vehicle_forward_speed, 0, dt); // Feedback
+		_pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint);
+		forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt);
 
 		// Closed loop lateral speed control
 		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
@@ -138,8 +128,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);
 		}
 
-		lateral_throttle += pid_calculate(&_pid_lateral_throttle, _rover_mecanum_setpoint.lateral_speed_setpoint,
-						  vehicle_lateral_speed, 0, dt); // Feedback
+		_pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint);
+		lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt);
 
 	} else { // Use normalized setpoint
 		forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain(
@@ -156,10 +146,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.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.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_pub.publish(rover_mecanum_status);
 
 	// Publish to motors
@@ -213,8 +203,8 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr
 
 void RoverMecanumControl::resetControllers()
 {
-	pid_reset_integral(&_pid_forward_throttle);
-	pid_reset_integral(&_pid_lateral_throttle);
-	pid_reset_integral(&_pid_yaw_rate);
-	pid_reset_integral(&_pid_yaw);
+	_pid_forward_throttle.resetIntegral();
+	_pid_lateral_throttle.resetIntegral();
+	_pid_yaw_rate.resetIntegral();
+	_pid_yaw.resetIntegral();
 }

src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp

@@ -46,7 +46,7 @@
 
 
 // Standard libraries
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #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_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
+	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
 
 	// Parameters
 	DEFINE_PARAMETERS(

src/modules/rover_pos_control/CMakeLists.txt

@@ -38,5 +38,5 @@ px4_add_module(
 		RoverPositionControl.hpp
 	DEPENDS
 		l1
-		pid
+		PID
 	)

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -92,13 +92,9 @@ void RoverPositionControl::parameters_update(bool force)
 		_gnd_control.set_l1_damping(_param_l1_damping.get());
 		_gnd_control.set_l1_period(_param_l1_period.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());
+		_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());
 	}
 }
 
@@ -239,12 +235,9 @@ 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
-			mission_throttle = _param_throttle_speed_scaler.get()
-					   * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
+			_speed_ctrl.setSetpoint(mission_target_speed);
+			mission_throttle = _param_throttle_speed_scaler.get() * _speed_ctrl.update(vel(0), dt);
 
 			// Constrain throttle between min and max
 			mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get());
@@ -327,10 +320,8 @@ 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));
 
-		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);
+		_speed_ctrl.setSetpoint(desired_speed);
+		const float control_throttle = _speed_ctrl.update(vel(0), dt);
 
 		//Constrain maximum throttle to mission throttle
 		_throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle);
@@ -392,8 +383,6 @@ RoverPositionControl::Run()
 		vehicle_attitude_poll();
 		manual_control_setpoint_poll();
 
-		_vehicle_acceleration_sub.update();
-
 		/* update parameters from storage */
 		parameters_update();
 

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -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.h>
+#include <lib/pid/PID.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
@@ -128,8 +128,6 @@ 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  */
@@ -139,7 +137,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_t _speed_ctrl{};
+	PID _speed_ctrl{};
 
 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};		// captures the number of times the estimator has reset the horizontal position

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -36,7 +36,7 @@ px4_add_library(vehicle_acceleration
 	VehicleAcceleration.hpp
 )
 
-target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPACE})
 
 target_link_libraries(vehicle_acceleration
 	PRIVATE

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -38,7 +38,7 @@ px4_add_library(vehicle_angular_velocity
 
 target_compile_options(vehicle_angular_velocity
 	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPACE}
 		#-DDEBUG_BUILD
 )
 

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -38,7 +38,7 @@ px4_add_library(vehicle_imu
 
 target_compile_options(vehicle_imu
 	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPACE}
 		#-DDEBUG_BUILD
 )
 

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -51,7 +51,7 @@ if(gz-transport_FOUND)
 		MODULE modules__simulation__gz_bridge
 		MAIN gz_bridge
 		COMPILE_FLAGS
-			${MAX_CUSTOM_OPT_LEVEL}
+			${MAX_CUSTOM_OPT_LEVEL_SPEED}
 		SRCS
 			GZBridge.cpp
 			GZBridge.hpp

src/modules/simulation/simulator_sih/CMakeLists.txt

@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__simulation__simulator_sih
 	MAIN simulator_sih
 	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
+		${MAX_CUSTOM_OPT_LEVEL_SPEED}
 	SRCS
 		aero.hpp
 		sih.cpp

src/modules/uuv_att_control/uuv_att_control.hpp

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

src/modules/uuv_pos_control/uuv_pos_control.hpp

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

src/modules/uxrce_dds_client/CMakeLists.txt

@@ -138,7 +138,7 @@ else()
 		COMPILE_FLAGS
 			#-O0
 			# -DDEBUG_BUILD
-			${MAX_CUSTOM_OPT_LEVEL}
+			${MAX_CUSTOM_OPT_LEVEL_SPACE}
 		SRCS
 			${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h
 			uxrce_dds_client.cpp