diff --git a/CMakeLists.txt b/CMakeLists.txt
index 656dd322ab..7ea47640f4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -181,6 +181,18 @@ if(NOT CMAKE_BUILD_TYPE)
 	set(CMAKE_BUILD_TYPE ${PX4_BUILD_TYPE} CACHE STRING "Build type" FORCE)
 endif()
 
+if((CMAKE_BUILD_TYPE STREQUAL "Debug") OR (CMAKE_BUILD_TYPE STREQUAL "Coverage"))
+	set(MAX_CUSTOM_OPT_LEVEL -O0)
+elseif(CMAKE_BUILD_TYPE MATCHES "Sanitizer")
+	set(MAX_CUSTOM_OPT_LEVEL -O1)
+else()
+	if(px4_constrained_flash_build)
+		set(MAX_CUSTOM_OPT_LEVEL -Os)
+	else()
+		set(MAX_CUSTOM_OPT_LEVEL -O2)
+	endif()
+endif()
+
 set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage;AddressSanitizer;UndefinedBehaviorSanitizer")
 message(STATUS "cmake build type: ${CMAKE_BUILD_TYPE}")
 
diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake
index e9c908517b..bd6f7f6786 100644
--- a/boards/bitcraze/crazyflie/default.cmake
+++ b/boards/bitcraze/crazyflie/default.cmake
@@ -6,6 +6,7 @@ px4_add_board(
 	TOOLCHAIN arm-none-eabi
 	ARCHITECTURE cortex-m4
 	ROMFSROOT px4fmu_common
+	CONSTRAINED_FLASH
 	DRIVERS
 		barometer/lps25h
 		distance_sensor/vl53l0x
diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake
index 6cfea43e88..44bf1edf1b 100644
--- a/boards/holybro/durandal-v1/stackcheck.cmake
+++ b/boards/holybro/durandal-v1/stackcheck.cmake
@@ -7,7 +7,7 @@ px4_add_board(
 	TOOLCHAIN arm-none-eabi
 	ARCHITECTURE cortex-m7
 	ROMFSROOT px4fmu_common
-	BUILD_BOOTLOADER
+	#BUILD_BOOTLOADER
 	IO px4_io-v2_default
 	TESTING
 #	UAVCAN_INTERFACES 2  - No H7 or FD can support in UAVCAN
@@ -23,7 +23,7 @@ px4_add_board(
 		batt_smbus
 		#camera_capture
 		#camera_trigger
-		differential_pressure # all available differential pressure drivers
+		#differential_pressure # all available differential pressure drivers
 		distance_sensor # all available distance sensor drivers
 		#dshot
 		gps
@@ -65,6 +65,7 @@ px4_add_board(
 		commander
 		dataman
 		ekf2
+		#esc_battery
 		events
 		fw_att_control
 		fw_pos_control_l1
@@ -78,16 +79,17 @@ px4_add_board(
 		mc_hover_thrust_estimator
 		mc_pos_control
 		mc_rate_control
+		#micrortps_bridge
 		navigator
 		rc_update
 		#rover_pos_control
 		sensors
 		#sih
-		temperature_compensation
-		vmount
+		#temperature_compensation
+		#vmount
 		vtol_att_control
 	SYSTEMCMDS
-		bl_update
+		#bl_update
 		dmesg
 		dumpfile
 		esc_calib
diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake
index b547d039da..64703e3984 100644
--- a/boards/holybro/kakutef7/default.cmake
+++ b/boards/holybro/kakutef7/default.cmake
@@ -7,6 +7,7 @@ px4_add_board(
 	TOOLCHAIN arm-none-eabi
 	ARCHITECTURE cortex-m7
 	ROMFSROOT px4fmu_common
+	CONSTRAINED_FLASH
 	SERIAL_PORTS
 		TEL1:/dev/ttyS0 # UART1
 		TEL2:/dev/ttyS1 # UART2
diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake
index a8b6bb4afe..779e072add 100644
--- a/boards/px4/fmu-v5/stackcheck.cmake
+++ b/boards/px4/fmu-v5/stackcheck.cmake
@@ -47,7 +47,7 @@ px4_add_board(
 		#pca9685
 		#power_monitor/ina226
 		#protocol_splitter
-		pwm_input
+		#pwm_input
 		pwm_out_sim
 		pwm_out
 		px4io
@@ -61,7 +61,7 @@ px4_add_board(
 		#uavcan
 	MODULES
 		airspeed_selector
-		attitude_estimator_q
+		#attitude_estimator_q
 		battery_status
 		#camera_feedback
 		commander
@@ -84,11 +84,11 @@ px4_add_board(
 		#micrortps_bridge
 		navigator
 		rc_update
-		rover_pos_control
+		#rover_pos_control
 		sensors
-		sih
-		temperature_compensation
-		vmount
+		#sih
+		#temperature_compensation
+		#vmount
 		vtol_att_control
 	SYSTEMCMDS
 		bl_update
diff --git a/boards/px4/io-v2/default.cmake b/boards/px4/io-v2/default.cmake
index d1144c1bd4..21956f2e9b 100644
--- a/boards/px4/io-v2/default.cmake
+++ b/boards/px4/io-v2/default.cmake
@@ -4,6 +4,7 @@ px4_add_board(
 	VENDOR px4
 	MODEL io-v2
 	TOOLCHAIN arm-none-eabi
+	CONSTRAINED_FLASH
 	ARCHITECTURE cortex-m3
 	DRIVERS
 	MODULES
diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
index 8f456c20b6..88e68f2fc1 100644
--- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
@@ -65,7 +65,7 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 1472, -11};
 static constexpr wq_config_t I2C4{"wq:I2C4", 1472, -12};
 
 // PX4 att/pos controllers, highest priority after sensors.
-static constexpr wq_config_t attitude_ctrl{"wq:attitude_ctrl", 1536, -13};
+static constexpr wq_config_t attitude_ctrl{"wq:attitude_ctrl", 1600, -13};
 static constexpr wq_config_t navigation_and_controllers{"wq:navigation_and_controllers", 7200, -14};
 
 static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -15};
diff --git a/platforms/common/px4_work_queue/CMakeLists.txt b/platforms/common/px4_work_queue/CMakeLists.txt
index a4e45a6a9a..429c69d6d1 100644
--- a/platforms/common/px4_work_queue/CMakeLists.txt
+++ b/platforms/common/px4_work_queue/CMakeLists.txt
@@ -43,4 +43,5 @@ if(PX4_TESTING)
 	add_subdirectory(test)
 endif()
 
+target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(px4_work_queue PRIVATE px4_platform)
diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt
index 124b1a1ae6..d60d949106 100644
--- a/platforms/nuttx/src/px4/common/CMakeLists.txt
+++ b/platforms/nuttx/src/px4/common/CMakeLists.txt
@@ -32,7 +32,7 @@
 ############################################################################
 
 # skip for px4_layer support on an IO board
-if (NOT ${PX4_BOARD} MATCHES "px4_io")
+if(NOT PX4_BOARD MATCHES "px4_io")
 
 	add_library(px4_layer
 		board_crashdump.c
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt
index d04316b3b7..5221ae896a 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt
+++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt
@@ -34,3 +34,4 @@
 px4_add_library(arch_dshot
 	dshot.c
 )
+target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/hrt/CMakeLists.txt
index ebd97f8a42..82854e0def 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/CMakeLists.txt
+++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/CMakeLists.txt
@@ -34,5 +34,8 @@
 px4_add_library(arch_hrt
 	hrt.c
 )
-target_compile_options(arch_hrt PRIVATE -Wno-cast-align) # TODO: fix and enable
-
+target_compile_options(arch_hrt
+	PRIVATE
+		${MAX_CUSTOM_OPT_LEVEL}
+		-Wno-cast-align # TODO: fix and enable
+)
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt
index 401bf06ef5..6314afc0a5 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt
+++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt
@@ -32,8 +32,9 @@
 ############################################################################
 
 px4_add_library(arch_io_pins
+	input_capture.c
 	io_timer.c
 	pwm_servo.c
 	pwm_trigger.c
-	input_capture.c
 )
+target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt
index 4f3e5d2e37..f20735002a 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt
+++ b/platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt
@@ -34,3 +34,4 @@
 px4_add_library(arch_spi
 	spi.cpp
 )
+target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
diff --git a/src/lib/drivers/accelerometer/CMakeLists.txt b/src/lib/drivers/accelerometer/CMakeLists.txt
index b3936435ae..7fcc4759d9 100644
--- a/src/lib/drivers/accelerometer/CMakeLists.txt
+++ b/src/lib/drivers/accelerometer/CMakeLists.txt
@@ -35,9 +35,5 @@ px4_add_library(drivers_accelerometer
 	PX4Accelerometer.cpp
 	PX4Accelerometer.hpp
 )
-target_link_libraries(drivers_accelerometer
-	PRIVATE
-		conversion
-		drivers__device
-		mathlib
-)
+target_compile_options(drivers_accelerometer PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_link_libraries(drivers_accelerometer PRIVATE conversion drivers__device)
diff --git a/src/lib/drivers/gyroscope/CMakeLists.txt b/src/lib/drivers/gyroscope/CMakeLists.txt
index 823fd21e1f..7a8c7edeb9 100644
--- a/src/lib/drivers/gyroscope/CMakeLists.txt
+++ b/src/lib/drivers/gyroscope/CMakeLists.txt
@@ -35,4 +35,5 @@ px4_add_library(drivers_gyroscope
 	PX4Gyroscope.cpp
 	PX4Gyroscope.hpp
 )
+target_compile_options(drivers_gyroscope PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(drivers_gyroscope PRIVATE conversion drivers__device)
diff --git a/src/lib/perf/CMakeLists.txt b/src/lib/perf/CMakeLists.txt
index ee474d8ecb..18c9481181 100644
--- a/src/lib/perf/CMakeLists.txt
+++ b/src/lib/perf/CMakeLists.txt
@@ -32,4 +32,5 @@
 ############################################################################
 
 add_library(perf perf_counter.cpp)
-add_dependencies(perf prebuild_targets)
\ No newline at end of file
+add_dependencies(perf prebuild_targets)
+target_compile_options(perf PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
diff --git a/src/lib/systemlib/CMakeLists.txt b/src/lib/systemlib/CMakeLists.txt
index 43216fe499..63255b25f5 100644
--- a/src/lib/systemlib/CMakeLists.txt
+++ b/src/lib/systemlib/CMakeLists.txt
@@ -33,13 +33,17 @@
 
 set(SRCS
 	conversions.c
+	conversions.h
 	crc.c
+	crc.h
 	mavlink_log.cpp
+	mavlink_log.h
 )
 
 if(${PX4_PLATFORM} STREQUAL "nuttx")
 	list(APPEND SRCS
 		cpuload.c
+		cpuload.h
 		print_load_nuttx.c
 		otp.c
 	)
@@ -50,3 +54,4 @@ else()
 endif()
 
 px4_add_library(systemlib ${SRCS})
+target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
diff --git a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
index 84110b6d4e..4ff754e8e9 100644
--- a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
+++ b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
@@ -33,10 +33,9 @@
 
 px4_add_library(AttitudeControl
 	AttitudeControl.cpp
+	AttitudeControl.hpp
 )
-target_include_directories(AttitudeControl
-	PUBLIC
-	${CMAKE_CURRENT_SOURCE_DIR}
-)
+target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(AttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl)
diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt
index 89952573e4..abd0f4ff9c 100644
--- a/src/modules/mc_att_control/CMakeLists.txt
+++ b/src/modules/mc_att_control/CMakeLists.txt
@@ -37,10 +37,12 @@ px4_add_module(
 	MODULE modules__mc_att_control
 	MAIN mc_att_control
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		mc_att_control_main.cpp
+		mc_att_control.hpp
 	DEPENDS
-		mathlib
 		AttitudeControl
+		mathlib
 		px4_work_queue
 	)
diff --git a/src/modules/mc_rate_control/CMakeLists.txt b/src/modules/mc_rate_control/CMakeLists.txt
index f2d2bdb04f..758d56b7c8 100644
--- a/src/modules/mc_rate_control/CMakeLists.txt
+++ b/src/modules/mc_rate_control/CMakeLists.txt
@@ -37,6 +37,7 @@ px4_add_module(
 	MODULE modules__mc_rate_control
 	MAIN mc_rate_control
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		MulticopterRateControl.cpp
 		MulticopterRateControl.hpp
diff --git a/src/modules/mc_rate_control/RateControl/CMakeLists.txt b/src/modules/mc_rate_control/RateControl/CMakeLists.txt
index 1cd08cb5b3..b536a7db09 100644
--- a/src/modules/mc_rate_control/RateControl/CMakeLists.txt
+++ b/src/modules/mc_rate_control/RateControl/CMakeLists.txt
@@ -33,12 +33,10 @@
 
 px4_add_library(RateControl
 	RateControl.cpp
+	RateControl.hpp
 )
-target_include_directories(RateControl
-	PUBLIC
-	${CMAKE_CURRENT_SOURCE_DIR}
-)
-
+target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(RateControl PRIVATE mathlib)
 
 px4_add_unit_gtest(SRC RateControlTest.cpp LINKLIBS RateControl)
diff --git a/src/modules/sensors/sensor_corrections/CMakeLists.txt b/src/modules/sensors/sensor_corrections/CMakeLists.txt
index 932e2029c5..35e53e6a96 100644
--- a/src/modules/sensors/sensor_corrections/CMakeLists.txt
+++ b/src/modules/sensors/sensor_corrections/CMakeLists.txt
@@ -35,3 +35,4 @@ px4_add_library(sensor_corrections
 	SensorCorrections.cpp
 	SensorCorrections.hpp
 )
+target_compile_options(sensor_corrections PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt
index 7a345b12fa..3411a86cc0 100644
--- a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt
+++ b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt
@@ -35,6 +35,9 @@ px4_add_library(vehicle_acceleration
 	VehicleAcceleration.cpp
 	VehicleAcceleration.hpp
 )
+
+target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+
 target_link_libraries(vehicle_acceleration
 	PRIVATE
 		mathlib
diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt
index a32fa705bb..c784479263 100644
--- a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt
+++ b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt
@@ -35,6 +35,9 @@ px4_add_library(vehicle_angular_velocity
 	VehicleAngularVelocity.cpp
 	VehicleAngularVelocity.hpp
 )
+
+target_compile_options(vehicle_angular_velocity PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+
 target_link_libraries(vehicle_angular_velocity
 	PRIVATE
 		mathlib
diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt
index 7ce66b836b..97b1006c57 100644
--- a/src/modules/sensors/vehicle_imu/CMakeLists.txt
+++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt
@@ -37,4 +37,5 @@ px4_add_library(vehicle_imu
 	VehicleIMU.cpp
 	VehicleIMU.hpp
 )
+target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(vehicle_imu PRIVATE sensor_corrections px4_work_queue)
diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt
index b06add28a6..48d0f94131 100644
--- a/src/modules/uORB/CMakeLists.txt
+++ b/src/modules/uORB/CMakeLists.txt
@@ -31,7 +31,7 @@
 #
 ############################################################################
 
-if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to platform layer)
+if(NOT PX4_BOARD MATCHES "px4_io") # TODO: fix this hack (move uORB to platform layer)
 
 	# this includes the generated topics directory
 	include_directories(${CMAKE_CURRENT_BINARY_DIR})
@@ -39,6 +39,8 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
 	px4_add_module(
 		MODULE modules__uORB
 		MAIN uorb
+		COMPILE_FLAGS
+			${MAX_CUSTOM_OPT_LEVEL}
 		SRCS
 			ORBSet.hpp
 			Publication.hpp