diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index 5a7f8390ff..b38f40eb4c 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -80,14 +80,22 @@ ARCHCPUFLAGS_CORTEXM3	 = -mcpu=cortex-m3 \
 			   -march=armv7-m \
 			   -mfloat-abi=soft
 
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
-			   -ffixed-r10
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10
 
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
-			   -ffixed-r10
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10
 
 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
 
+# Enabling stack checks if requested
+#
+ENABLE_STACK_CHECKS ?= 0
+ifneq ($(ENABLE_STACK_CHECKS),0)
+$(info Stack checks enabled)
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4F += -finstrument-functions
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4  += -finstrument-functions
+ARCHINSTRUMENTATIONDEFINES_CORTEXM3  +=
+endif
+
 # Pick the right set of flags for the architecture.
 #
 ARCHCPUFLAGS		 = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index 4d1b7156b8..a3c6e46a0a 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -40,7 +40,8 @@ MODULE_COMMAND = uavcan
 
 MAXOPTIMIZATION = -O3
 
-INSTRUMENTATIONDEFINES = -fno-instrument-functions -ffixed-r10
+# Instrumentation makes the CPU load about 3 times higher, see https://github.com/PX4/Firmware/issues/1417
+INSTRUMENTATIONDEFINES += -fno-instrument-functions
 
 # Main
 SRCS += uavcan_main.cpp              \