diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk
index 41e8b95ff5..300afa3d5b 100644
--- a/makefiles/config_px4fmu-v1_test.mk
+++ b/makefiles/config_px4fmu-v1_test.mk
@@ -29,6 +29,9 @@ MODULES		+= systemcmds/nshterm
 MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/uORB
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
 
 #
 # Transitional support - add commands from the NuttX export archive.
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index f54a4d8257..ea52213df0 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -18,6 +18,12 @@ MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/led
 MODULES		+= drivers/boards/px4fmu-v2
 MODULES		+= drivers/px4io
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/tests
@@ -30,6 +36,9 @@ MODULES		+= systemcmds/mtd
 MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/uORB
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
 
 #
 # Transitional support - add commands from the NuttX export archive.