diff --git a/stm-firmware/CMakeLists.txt b/stm-firmware/CMakeLists.txt
index 5c82698..614812d 100644
--- a/stm-firmware/CMakeLists.txt
+++ b/stm-firmware/CMakeLists.txt
@@ -49,13 +49,17 @@ message("${BoldGreen}Version: ${GIT_DESCRIBE}${ColorReset}")
 
 IF(CMAKE_BUILD_TYPE STREQUAL "Debug")
 add_definitions(-DDEBUGBUILD)
-add_compile_options(-O0 -g)
+add_compile_options(-Og -g)
 ELSE()
-add_definitions(-DDEBUGBUILD)
 add_compile_options(-O3 -g)
 add_link_options(-Wl,--gc-sections)
 ENDIF(CMAKE_BUILD_TYPE STREQUAL "Debug")
 
+
+if (DEFINED UART_ON_DEBUG_HEADER)
+    add_definitions(-DUART_ON_DEBUG_HEADER)
+endif (DEFINED UART_ON_DEBUG_HEADER)
+
 add_subdirectory(base64-lib)