diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1526bbc1e5..a713141b61 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -318,14 +318,15 @@ px4_os_add_flags(
 	BOARD ${BOARD}
 	C_FLAGS c_flags
 	CXX_FLAGS cxx_flags
+	OPTIMIZATION_FLAGS optimization_flags
 	EXE_LINKER_FLAGS exe_linker_flags
 	INCLUDE_DIRS include_dirs
 	LINK_DIRS link_dirs
 	DEFINITIONS definitions)
 
 px4_join(OUT CMAKE_EXE_LINKER_FLAGS LIST "${exe_linker_flags}" GLUE " ")
-px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags}" GLUE " ")
-px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
+px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags};${optimization_flags}" GLUE " ")
+px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags};${optimization_flags}" GLUE " ")
 
 include_directories(${include_dirs})
 #message("INCLUDE_DIRS=${include_dirs}")
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index c30d79d6c7..6b5bbec1c5 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -45,6 +45,9 @@
 #		* px4_generate_messages
 #		* px4_add_upload
 #		* px4_add_common_flags
+#		* px4_add_optimization_flags_for_target
+#		* px4_add_executable
+#		* px4_add_library
 #
 
 include(CMakeParseArguments)
@@ -263,7 +266,7 @@ function(px4_add_module)
 		REQUIRED MODULE
 		ARGN ${ARGN})
 
-	add_library(${MODULE} STATIC EXCLUDE_FROM_ALL ${SRCS})
+	px4_add_library(${MODULE} STATIC EXCLUDE_FROM_ALL ${SRCS})
 
 	# set defaults if not set
 	set(MAIN_DEFAULT MAIN-NOTFOUND)
@@ -323,6 +326,9 @@ function(px4_add_module)
 	# store module properties in target
 	# COMPILE_FLAGS and LINK_FLAGS are passed to compiler/linker by cmake
 	# STACK_MAIN, MAIN, PRIORITY are PX4 specific
+	if(COMPILE_FLAGS AND ${_no_optimization_for_target})
+		px4_strip_optimization(COMPILE_FLAGS ${COMPILE_FLAGS})
+	endif()
 	foreach (prop COMPILE_FLAGS LINK_FLAGS STACK_MAIN MAIN PRIORITY)
 		if (${prop})
 			set_target_properties(${MODULE} PROPERTIES ${prop} ${${prop}})
@@ -445,7 +451,7 @@ function(px4_generate_messages)
 		VERBATIM
 		)
 
-	add_library(${TARGET}
+	px4_add_library(${TARGET}
 		${msg_source_files_out}
 		${msg_multi_files_out}
 		${msg_files_out}
@@ -577,6 +583,7 @@ endfunction()
 #			BOARD <in-string>
 #			C_FLAGS <inout-variable>
 #			CXX_FLAGS <inout-variable>
+#			OPTIMIZATION_FLAGS <inout-variable>
 #			EXE_LINKER_FLAGS <inout-variable>
 #			INCLUDE_DIRS <inout-variable>
 #			LINK_DIRS <inout-variable>
@@ -588,6 +595,7 @@ endfunction()
 #	Input/Output: (appends to existing variable)
 #		C_FLAGS					: c compile flags variable
 #		CXX_FLAGS				: c++ compile flags variable
+#		OPTIMIZATION_FLAGS			: optimization compile flags variable
 #		EXE_LINKER_FLAGS			: executable linker flags variable
 #		INCLUDE_DIRS				: include directories
 #		LINK_DIRS				: link directories
@@ -598,13 +606,14 @@ endfunction()
 #			BOARD px4fmu-v2
 #			C_FLAGS CMAKE_C_FLAGS
 #			CXX_FLAGS CMAKE_CXX_FLAGS
+#			OPTIMIZATION_FLAGS optimization_flags
 #			EXE_LINKER_FLAG CMAKE_EXE_LINKER_FLAGS
 #			INCLUDES <list>)
 #
 function(px4_add_common_flags)
 
 	set(inout_vars
-		C_FLAGS CXX_FLAGS EXE_LINKER_FLAGS INCLUDE_DIRS LINK_DIRS DEFINITIONS)
+		C_FLAGS CXX_FLAGS OPTIMIZATION_FLAGS EXE_LINKER_FLAGS INCLUDE_DIRS LINK_DIRS DEFINITIONS)
 
 	px4_parse_function_args(
 		NAME px4_add_common_flags
@@ -660,7 +669,8 @@ function(px4_add_common_flags)
 		message(STATUS "address sanitizer enabled")
 		set(max_optimization -Os)
 
-		set(optimization_flags
+		# Do not use optimization_flags (without _) as that is already used.
+		set(_optimization_flags
 			-fno-strict-aliasing
 			-fno-omit-frame-pointer
 			-funsafe-math-optimizations
@@ -674,7 +684,7 @@ function(px4_add_common_flags)
 		if ("${OS}" STREQUAL "qurt")
 			set(PIC_FLAG -fPIC)
 		endif()
-		set(optimization_flags
+		set(_optimization_flags
 			-fno-strict-aliasing
 			-fomit-frame-pointer
 			-funsafe-math-optimizations
@@ -685,7 +695,7 @@ function(px4_add_common_flags)
 	endif()
 
 	if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*")
-		list(APPEND optimization_flags
+		list(APPEND _optimization_flags
 			-fno-strength-reduce
 			-fno-builtin-printf
 		)
@@ -743,8 +753,6 @@ function(px4_add_common_flags)
 		${c_compile_flags}
 		${warnings}
 		${c_warnings}
-		${max_optimization}
-		${optimization_flags}
 		${visibility_flags}
 		)
 
@@ -752,11 +760,14 @@ function(px4_add_common_flags)
 		${cxx_compile_flags}
 		${warnings}
 		${cxx_warnings}
-		${max_optimization}
-		${optimization_flags}
 		${visibility_flags}
 		)
 
+	set(added_optimization_flags
+		${max_optimization}
+		${_optimization_flags}
+		)
+
 	set(added_include_dirs
 		${PX4_SOURCE_DIR}/src
 		${PX4_BINARY_DIR}
@@ -1030,5 +1041,66 @@ function(px4_copy_tracked)
 	set(${OUT} ${_files_out} PARENT_SCOPE)
 endfunction()
 
+#=============================================================================
+#
+#	px4_strip_optimization
+#
+function(px4_strip_optimization name)
+	set(_compile_flags)
+	separate_arguments(_args UNIX_COMMAND ${ARGN})
+	foreach(_flag ${_args})
+		if(NOT "${_flag}" MATCHES "^-O")
+			set(_compile_flags "${_compile_flags} ${_flag}")
+		endif()
+	endforeach()
+	string(STRIP "${_compile_flags}" _compile_flags)
+	set(${name} "${_compile_flags}" PARENT_SCOPE)
+endfunction()
+
+#=============================================================================
+#
+#	px4_add_optimization_flags_for_target
+#
+function(px4_add_optimization_flags_for_target target)
+	set(_no_optimization_for_target FALSE)
+	foreach(_regexp $ENV{PX4_NO_OPTIMIZATION})
+		if("${target}" MATCHES "${_regexp}")
+			set(_no_optimization_for_target TRUE)
+			set(_matched_regexp "${_regexp}")
+		endif()
+	endforeach()
+	if(NOT ${_no_optimization_for_target})
+		target_compile_options(${target} PRIVATE ${optimization_flags})
+	else()
+		message(STATUS "Disabling optimization for target '${target}' because it matches the regexp '${_matched_regexp}' in env var PX4_NO_OPTIMIZATION")
+		target_compile_options(${target} PRIVATE -O0)
+	endif()
+	# Pass variable to the parent px4_add_library.
+	set(_no_optimization_for_target ${_no_optimization_for_target} PARENT_SCOPE)
+endfunction()
+
+#=============================================================================
+#
+#	px4_add_executable
+#
+#	Like add_executable but with optimization flag fixup.
+#
+function(px4_add_executable target)
+	add_executable(${target} ${ARGN})
+	px4_add_optimization_flags_for_target(${target})
+endfunction()
+
+#=============================================================================
+#
+#	px4_add_library
+#
+#	Like add_library but with optimization flag fixup.
+#
+function(px4_add_library target)
+	add_library(${target} ${ARGN})
+	px4_add_optimization_flags_for_target(${target})
+	# Pass variable to the parent px4_add_module.
+	set(_no_optimization_for_target ${_no_optimization_for_target} PARENT_SCOPE)
+endfunction()
 
 # vim: set noet fenc=utf-8 ff=unix nowrap:
diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index 55e87ac8f3..757868f488 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -371,6 +371,7 @@ endfunction()
 #		px4_os_add_flags(
 #			C_FLAGS <inout-variable>
 #			CXX_FLAGS <inout-variable>
+#			OPTIMIZATION_FLAGS <inout-variable>
 #			EXE_LINKER_FLAGS <inout-variable>
 #			INCLUDE_DIRS <inout-variable>
 #			LINK_DIRS <inout-variable>
@@ -382,6 +383,7 @@ endfunction()
 #	Input/Output: (appends to existing variable)
 #		C_FLAGS					: c compile flags variable
 #		CXX_FLAGS				: c++ compile flags variable
+#		OPTIMIZATION_FLAGS			: optimization compile flags variable
 #		EXE_LINKER_FLAGS			: executable linker flags variable
 #		INCLUDE_DIRS				: include directories
 #		LINK_DIRS				: link directories
@@ -391,13 +393,14 @@ endfunction()
 #		px4_os_add_flags(
 #			C_FLAGS CMAKE_C_FLAGS
 #			CXX_FLAGS CMAKE_CXX_FLAGS
+#			OPTIMIZATION_FLAGS optimization_flags
 #			EXE_LINKER_FLAG CMAKE_EXE_LINKER_FLAGS
 #			INCLUDES <list>)
 #
 function(px4_os_add_flags)
 
 	set(inout_vars
-		C_FLAGS CXX_FLAGS EXE_LINKER_FLAGS INCLUDE_DIRS LINK_DIRS DEFINITIONS)
+		C_FLAGS CXX_FLAGS OPTIMIZATION_FLAGS EXE_LINKER_FLAGS INCLUDE_DIRS LINK_DIRS DEFINITIONS)
 
 	px4_parse_function_args(
 		NAME px4_os_add_flags
@@ -409,6 +412,7 @@ function(px4_os_add_flags)
 		BOARD ${BOARD}
 		C_FLAGS ${C_FLAGS}
 		CXX_FLAGS ${CXX_FLAGS}
+		OPTIMIZATION_FLAGS ${OPTIMIZATION_FLAGS}
 		EXE_LINKER_FLAGS ${EXE_LINKER_FLAGS}
 		INCLUDE_DIRS ${INCLUDE_DIRS}
 		LINK_DIRS ${LINK_DIRS}
diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake
index d5757f7247..dd329bd761 100644
--- a/cmake/posix/px4_impl_posix.cmake
+++ b/cmake/posix/px4_impl_posix.cmake
@@ -115,6 +115,7 @@ endfunction()
 #		px4_os_add_flags(
 #			C_FLAGS <inout-variable>
 #			CXX_FLAGS <inout-variable>
+#			OPTIMIZATION_FLAGS <inout-variable>
 #			EXE_LINKER_FLAGS <inout-variable>
 #			INCLUDE_DIRS <inout-variable>
 #			LINK_DIRS <inout-variable>
@@ -126,6 +127,7 @@ endfunction()
 #	Input/Output: (appends to existing variable)
 #		C_FLAGS					: c compile flags variable
 #		CXX_FLAGS				: c++ compile flags variable
+#		OPTIMIZATION_FLAGS			: optimization compile flags variable
 #		EXE_LINKER_FLAGS			: executable linker flags variable
 #		INCLUDE_DIRS				: include directories
 #		LINK_DIRS				: link directories
@@ -135,13 +137,14 @@ endfunction()
 #		px4_os_add_flags(
 #			C_FLAGS CMAKE_C_FLAGS
 #			CXX_FLAGS CMAKE_CXX_FLAGS
+#			OPTIMIZATION_FLAGS optimization_flags
 #			EXE_LINKER_FLAG CMAKE_EXE_LINKER_FLAGS
 #			INCLUDES <list>)
 #
 function(px4_os_add_flags)
 
 	set(inout_vars
-		C_FLAGS CXX_FLAGS EXE_LINKER_FLAGS INCLUDE_DIRS LINK_DIRS DEFINITIONS)
+		C_FLAGS CXX_FLAGS OPTIMIZATION_FLAGS EXE_LINKER_FLAGS INCLUDE_DIRS LINK_DIRS DEFINITIONS)
 
 	px4_parse_function_args(
 		NAME px4_os_add_flags
@@ -153,6 +156,7 @@ function(px4_os_add_flags)
 		BOARD ${BOARD}
 		C_FLAGS ${C_FLAGS}
 		CXX_FLAGS ${CXX_FLAGS}
+		OPTIMIZATION_FLAGS ${OPTIMIZATION_FLAGS}
 		EXE_LINKER_FLAGS ${EXE_LINKER_FLAGS}
 		INCLUDE_DIRS ${INCLUDE_DIRS}
 		LINK_DIRS ${LINK_DIRS}
diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake
index 118a9a8caa..b9e1d426d6 100644
--- a/cmake/qurt/px4_impl_qurt.cmake
+++ b/cmake/qurt/px4_impl_qurt.cmake
@@ -108,6 +108,7 @@ endfunction()
 #		px4_os_add_flags(
 #			C_FLAGS <inout-variable>
 #			CXX_FLAGS <inout-variable>
+#			OPTIMIZATION_FLAGS <inout-variable>
 #			EXE_LINKER_FLAGS <inout-variable>
 #			INCLUDE_DIRS <inout-variable>
 #			LINK_DIRS <inout-variable>
@@ -119,6 +120,7 @@ endfunction()
 #	Input/Output: (appends to existing variable)
 #		C_FLAGS					: c compile flags variable
 #		CXX_FLAGS				: c++ compile flags variable
+#		OPTIMIZATION_FLAGS			: optimization compile flags variable
 #		EXE_LINKER_FLAGS			: executable linker flags variable
 #		INCLUDE_DIRS				: include directories
 #		LINK_DIRS				: link directories
@@ -128,13 +130,14 @@ endfunction()
 #		px4_os_add_flags(
 #			C_FLAGS CMAKE_C_FLAGS
 #			CXX_FLAGS CMAKE_CXX_FLAGS
+#			OPTIMIZATION_FLAGS optimization_flags
 #			EXE_LINKER_FLAG CMAKE_EXE_LINKER_FLAGS
 #			INCLUDES <list>)
 #
 function(px4_os_add_flags)
 
 	set(inout_vars
-		C_FLAGS CXX_FLAGS EXE_LINKER_FLAGS INCLUDE_DIRS LINK_DIRS DEFINITIONS)
+		C_FLAGS CXX_FLAGS OPTIMIZATION_FLAGS EXE_LINKER_FLAGS INCLUDE_DIRS LINK_DIRS DEFINITIONS)
 
 	px4_parse_function_args(
 		NAME px4_os_add_flags
@@ -146,6 +149,7 @@ function(px4_os_add_flags)
 		BOARD ${BOARD}
 		C_FLAGS ${C_FLAGS}
 		CXX_FLAGS ${CXX_FLAGS}
+		OPTIMIZATION_FLAGS ${OPTIMIZATION_FLAGS}
 		EXE_LINKER_FLAGS ${EXE_LINKER_FLAGS}
 		INCLUDE_DIRS ${INCLUDE_DIRS}
 		LINK_DIRS ${LINK_DIRS}
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index e761f12220..30cb45cb71 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -39,7 +39,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 
 elseif ("${BOARD}" STREQUAL "rpi")
 
-	add_executable(px4
+	px4_add_executable(px4
 		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
@@ -66,7 +66,7 @@ elseif ("${BOARD}" STREQUAL "bebop")
 	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -static")
 	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -static")
 
-	add_executable(px4
+	px4_add_executable(px4
 		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
@@ -96,10 +96,11 @@ elseif ("${BOARD}" STREQUAL "bebop")
 
 else()
 
-	add_executable(px4
+	px4_add_executable(px4
 		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
+
 	if (NOT APPLE)
 		target_link_libraries(px4
 			-Wl,--start-group
diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt
index 67183999eb..a0a52ac9cf 100644
--- a/src/modules/px4iofirmware/CMakeLists.txt
+++ b/src/modules/px4iofirmware/CMakeLists.txt
@@ -40,6 +40,7 @@ set_directory_properties(PROPERTIES
 set(c_flags)
 set(exe_linker_flags)
 set(cxx_flags)
+set(optimization_flags)
 set(include_dirs)
 set(link_dirs)
 set(definitions)
@@ -52,14 +53,15 @@ px4_os_add_flags(
 	BOARD ${config_io_board}
 	C_FLAGS c_flags
 	CXX_FLAGS cxx_flags
+	OPTIMIZATION_FLAGS optimization_flags
 	EXE_LINKER_FLAGS exe_linker_flags
 	INCLUDE_DIRS include_dirs
 	LINK_DIRS link_dirs
 	DEFINITIONS definitions)
 
 px4_join(OUT CMAKE_EXE_LINKER_FLAGS LIST "${exe_linker_flags}" GLUE " ")
-px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags}" GLUE " ")
-px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
+px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags};${optimization_flags}" GLUE " ")
+px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags};${optimization_flags}" GLUE " ")
 
 include_directories(
 	${include_dirs}