diff --git a/Makefile b/Makefile
index 286bf7ad56..db775507ac 100644
--- a/Makefile
+++ b/Makefile
@@ -114,6 +114,12 @@ define cmake-build
 +@(echo "PX4 CONFIG: $@" && cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
+define cmake-build-other
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf $(PWD)/build_$@); fi
++@(cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
+endef
+
 # create empty targets to avoid msgs for targets passed to cmake
 define cmake-targ
 $(1):
@@ -219,8 +225,14 @@ run_sitl_ros: sitl_deprecation
 # Other targets
 # --------------------------------------------------------------------
 
-.PHONY: uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
-.NOTPARALLEL: uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
+.PHONY: gazebo_build uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
+.NOTPARALLEL: gazebo_build uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
+
+gazebo_build:
+	@mkdir -p build_gazebo
+	@if [ ! -e $(PWD)/build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) $(PWD)/Tools/sitl_gazebo; fi
+	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS)
+	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) sdf
 
 uavcan_firmware:
 ifeq ($(VECTORCONTROL),1)
@@ -251,7 +263,14 @@ ifeq ($(VECTORCONTROL),1)
 endif
 
 unittest: posix_sitl_test
-	@(mkdir -p build_unittests && cd build_unittests && cmake -G$(PX4_CMAKE_GENERATOR) ../unittests && $(PX4_MAKE) $(PX4_MAKE_ARGS) && ctest -j2 --output-on-failure)
+	export CC=clang
+	export CXX=clang++
+	export ASAN_OPTIONS=symbolize=1
+	$(call cmake-build-other,unittest, ../unittests)
+	@(cd build_unittest && ctest -j2 --output-on-failure)
+	
+test_onboard_sitl:
+	@HEADLESS=1 make posix_sitl_test gazebo_iris
 
 package_firmware:
 	@zip --junk-paths Firmware.zip `find . -name \*.px4`
diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh
index aa5bd9ae3f..fbb93c86fa 100755
--- a/Tools/sitl_run.sh
+++ b/Tools/sitl_run.sh
@@ -58,7 +58,7 @@ cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
 
 SIM_PID=0
 
-if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ]
+if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ]
 then
 	cd Tools/jMAVSim
 	ant create_run_jar copy_res
@@ -66,31 +66,33 @@ then
 	java -Djava.ext.dirs= -jar jmavsim_run.jar -udp 127.0.0.1:14560 &
 	SIM_PID=`echo $!`
 	cd ../..
-elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ]
+elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]
 then
 	if [ -x "$(command -v gazebo)" ]
 	then
 		# Set the plugin path so Gazebo finds our model and sim
-		export GAZEBO_PLUGIN_PATH=$curr_dir/Tools/sitl_gazebo/Build:${GAZEBO_PLUGIN_PATH}
+		export GAZEBO_PLUGIN_PATH=$curr_dir/build_gazebo:${GAZEBO_PLUGIN_PATH}
 		# Set the model path so Gazebo finds the airframes
 		export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models
 		# The next line would disable online model lookup, can be commented in, in case of unstable behaviour.
 		# export GAZEBO_MODEL_DATABASE_URI=""
 		export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo
-		mkdir -p Tools/sitl_gazebo/Build
-		cd Tools/sitl_gazebo/Build
-		cmake -Wno-dev ..
-		make -j4
-		make sdf
-		gzserver --verbose ../worlds/${model}.world &
+		make --no-print-directory gazebo_build
+
+		gzserver --verbose $curr_dir/Tools/sitl_gazebo/worlds/${model}.world &
 		SIM_PID=`echo $!`
-		gzclient --verbose &
-		GUI_PID=`echo $!`
+
+		if [[ -n "$HEADLESS" ]]; then
+			echo "not running gazebo gui"
+		else
+			gzclient --verbose &
+			GUI_PID=`echo $!`
+		fi
 	else
 		echo "You need to have gazebo simulator installed!"
 		exit 1
 	fi
-elif [ "$program" == "replay" ] && [ "$no_sim" == "" ]
+elif [ "$program" == "replay" ] && [ ! -n "$no_sim" ]
 then
 	echo "Replaying logfile: $logfile"
 	# This is not a simulator, but a log file to replay
@@ -135,5 +137,7 @@ then
 elif [ "$program" == "gazebo" ]
 then
 	kill -9 $SIM_PID
-	kill -9 $GUI_PID
+	if [[ ! -n "$HEADLESS" ]]; then
+		kill -9 $GUI_PID
+	fi
 fi
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
index e9fd6734cc..3f233985d5 100644
--- a/cmake/configs/posix_sitl_test.cmake
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -87,7 +87,7 @@ set(config_extra_builtin_cmds
 	)
 
 set(config_sitl_rcS
-	posix-configs/SITL/init/rcS
+	posix-configs/SITL/init/rcS_test
 	CACHE FILEPATH "init script for sitl"
 	)
 
diff --git a/posix-configs/SITL/init/rcS_test_gazebo_iris b/posix-configs/SITL/init/rcS_test_gazebo_iris
new file mode 100644
index 0000000000..8aeacf1eff
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_test_gazebo_iris
@@ -0,0 +1,25 @@
+uorb start
+param load
+
+dataman start
+
+simulator start -s
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sleep 1
+sensors start
+
+commander_tests
+controllib_test
+uorb_tests
+tests all
+
+ver all
+
+shutdown
diff --git a/posix-configs/SITL/init/rcS_test_jmavsim_iris b/posix-configs/SITL/init/rcS_test_jmavsim_iris
index b419960f24..8aeacf1eff 100644
--- a/posix-configs/SITL/init/rcS_test_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_test_jmavsim_iris
@@ -14,7 +14,6 @@ gpssim start
 pwm_out_sim mode_pwm
 sleep 1
 sensors start
-commander start
 
 commander_tests
 controllib_test
@@ -22,3 +21,5 @@ uorb_tests
 tests all
 
 ver all
+
+shutdown