diff --git a/Jenkinsfile b/Jenkinsfile
index 1b7ab879eb44a2d3861a8227ff14f3217239de98..37938711f28aea0b16b00a1942c2ee69e876ea34 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -46,6 +46,7 @@ pipeline {
                     sh "wget --no-verbose -N https://s3.amazonaws.com/px4-travis/Firmware/master/nuttx_px4fmu-v2_default.elf"
                     sh "bloaty -d symbols -n 100 -C full -s file build/nuttx_px4fmu-v2_default/nuttx_px4fmu-v2_default.elf -- nuttx_px4fmu-v2_default.elf"
                     sh "make nuttx_px4fmu-v2_lpe"
+                    sh "make nuttx_px4fmu-v2_test"
                     sh "make nuttx_px4fmu-v3_default"
                     sh "bloaty -d symbols -n 100 -s vm build/nuttx_px4fmu-v3_default/nuttx_px4fmu-v3_default.elf"
                     sh "bloaty -d compileunits -n 100 -s vm build/nuttx_px4fmu-v3_default/nuttx_px4fmu-v3_default.elf"
diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
index 07bee26fbf9b47908cb6fcb85aebbf9b3d51fe66..8d9661138bda3a87bae3717f0705d8bf651a229e 100644
--- a/ROMFS/px4fmu_test/init.d/rcS
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -2,6 +2,7 @@
 #
 # PX4FMU startup script for test hackery.
 #
+set +e
 set unit_test_failure 0
 
 uorb start
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 77ecc708fce899538e79f082d16693d6a74ff0f3..5b8ca4329474b46fe58f09a1f61d2a7da23dc7fa 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -1,59 +1,71 @@
+px4_nuttx_configure(HWCLASS m4 CONFIG nsh ROMFS y ROMFSROOT px4fmu_test IO px4io-v2)
 
-px4_nuttx_configure(HWCLASS m4 CONFIG nsh ROMFS y ROMFSROOT px4fmu_test)
+#set(config_uavcan_num_ifaces 2)
 
 set(config_module_list
 	#
 	# Board support modules
 	#
+	#drivers/barometer
+	drivers/differential_pressure
+	#drivers/magnetometer
+	#drivers/telemetry
+
+	#drivers/imu/adis16448
+	drivers/airspeed
+	drivers/barometer/ms5611
+	#drivers/blinkm
+	#drivers/imu/bmi160
+	#drivers/barometer/bmp280
+	drivers/boards
+	#drivers/bst
+	#drivers/camera_trigger
 	drivers/device
-	drivers/stm32
-	drivers/stm32/adc
-	drivers/stm32/tone_alarm
+	#drivers/frsky_telemetry
+	drivers/gps
+	#drivers/hott
+	#drivers/iridiumsbd
+	#drivers/irlock
+	drivers/imu/l3gd20
 	drivers/led
-	drivers/px4fmu
-	drivers/px4io
-	drivers/boards
-	drivers/rgbled
-	drivers/imu/mpu6000
-#TO FIT	drivers/imu/mpu9250
 	drivers/imu/lsm303d
-	drivers/imu/l3gd20
-	drivers/hmc5883
+	drivers/magnetometer/hmc5883
+	drivers/magnetometer/lis3mdl
 	#drivers/mb12xx
-	#drivers/srf02
-	#drivers/sf0x
-	#drivers/ll40ls
-	#drivers/teraranger
-	drivers/gps
-	#drivers/pwm_out_sim
-	#drivers/hott
-	drivers/blinkm
-	drivers/airspeed
-	drivers/barometer
-	drivers/differential_pressure
-	modules/sensors
 	#drivers/mkblctrl
-	drivers/px4flow
+	drivers/imu/mpu6000
+	#drivers/imu/mpu9250
 	#drivers/oreoled
+	#drivers/protocol_splitter
+	#drivers/pwm_input
+	#drivers/pwm_out_sim
+	#drivers/px4flow
+	drivers/px4fmu
+	drivers/px4io
+	drivers/rgbled
+	drivers/stm32
+	drivers/stm32/adc
+	drivers/stm32/tone_alarm
+	#drivers/tap_esc
 	#drivers/vmount
-	drivers/pwm_input
-	drivers/camera_trigger
-	#drivers/bst
-	#drivers/lis3mdl
-	drivers/tfmini
+
+	# distance sensors
+	modules/sensors
 
 	#
 	# System commands
 	#
-	systemcmds/bl_update
-	systemcmds/config
-	systemcmds/dumpfile
+	#systemcmds/bl_update
+	#systemcmds/config
+	#systemcmds/dumpfile
 	#systemcmds/esc_calib
-#TO FIT	systemcmds/hardfault_log
+	systemcmds/hardfault_log
+	#systemcmds/led_control
 	systemcmds/mixer
-	systemcmds/motor_ramp
+	#systemcmds/motor_ramp
+	#systemcmds/motor_test
 	systemcmds/mtd
-	systemcmds/nshterm
+	#systemcmds/nshterm
 	systemcmds/param
 	systemcmds/perf
 	systemcmds/pwm
@@ -61,6 +73,7 @@ set(config_module_list
 	#systemcmds/sd_bench
 	systemcmds/top
 	#systemcmds/topic_listener
+	systemcmds/tune_control
 	systemcmds/ver
 
 	#
@@ -68,41 +81,47 @@ set(config_module_list
 	#
 	drivers/distance_sensor/sf0x/sf0x_tests
 	drivers/test_ppm
+	lib/controllib/controllib_test
 	#lib/rc/rc_tests
 	modules/commander/commander_tests
-	modules/mc_pos_control/mc_pos_control_tests
-	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
+	modules/mc_pos_control/mc_pos_control_tests
 	modules/uORB/uORB_tests
 	systemcmds/tests
 
 	#
 	# General system control
 	#
+	modules/camera_feedback
 	modules/commander
+	modules/events
+	#modules/gpio_led
+	modules/land_detector
 	modules/load_mon
-	modules/navigator
 	modules/mavlink
-	#modules/gpio_led
+	#modules/navigator
 	#modules/uavcan
-	modules/land_detector
 
 	#
 	# Estimation modules
 	#
 	#modules/attitude_estimator_q
-	#modules/position_estimator_inav
+	#modules/ekf2
 	#modules/local_position_estimator
-	modules/ekf2
+	#modules/position_estimator_inav
+	#modules/landing_target_estimator
+	#modules/wind_estimator
 
 	#
 	# Vehicle Control
 	#
-	modules/fw_pos_control_l1
-	modules/fw_att_control
-	modules/mc_att_control
+	#modules/fw_att_control
+	#modules/fw_pos_control_l1
+	#modules/gnd_att_control
+	#modules/gnd_pos_control
+	#modules/mc_att_control
 	modules/mc_pos_control
-	modules/vtol_att_control
+	#modules/vtol_att_control
 
 	#
 	# Logging
@@ -113,10 +132,10 @@ set(config_module_list
 	#
 	# Library modules
 	#
-	modules/systemlib/param
+	modules/dataman
 	modules/systemlib
+	modules/systemlib/param
 	modules/uORB
-	modules/dataman
 
 	#
 	# Libraries
@@ -128,7 +147,7 @@ set(config_module_list
 	lib/led
 	lib/mathlib
 	lib/mixer
-	lib/terrain_estimation
+	#lib/terrain_estimation
 	lib/tunes
 	lib/version
 	lib/FlightTasks
@@ -143,10 +162,15 @@ set(config_module_list
 	#
 	#examples/rover_steering_control
 
+	#
+	# Segway
+	#
+	#examples/segway
+
 	#
 	# Demo apps
 	#
-	#examples/math_demo
+
 	# Tutorial code from
 	# https://px4.io/dev/px4_simple_app
 	#examples/px4_simple_app