diff --git a/CMakeLists.txt b/CMakeLists.txt
index d1815f6d8c44..6e6be184abe9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-# Copyright (c) 2017 PX4 Development Team. All rights reserved.
+# Copyright (c) 2017 - 2019 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -299,10 +299,6 @@ if (${PX4_PLATFORM} STREQUAL "posix")
 	if (NOT CMAKE_INSTALL_PREFIX)
 		set(CMAKE_INSTALL_PREFIX "/usr" CACHE PATH "Install path prefix" FORCE)
 	endif()
-
-	# cmake testing only on posix
-	enable_testing()
-	include(CTest)
 endif()
 
 #=============================================================================
@@ -413,6 +409,37 @@ if (NOT EXTERNAL_MODULES_LOCATION STREQUAL "")
 	endforeach()
 endif()
 
+#=============================================================================
+# Testing - Automatic unit and integration testing with CTest
+#
+
+# optionally enable cmake testing (supported only on posix)
+option(CMAKE_TESTING "Configure test targets" OFF)
+if(CMAKE_TESTING)
+	include(CTest) # sets BUILD_TESTING variable
+endif()
+
+# enable test filtering to run only specific tests with the ctest -R regex functionality
+set(TESTFILTER "" CACHE STRING "Filter string for ctest to selectively only run specific tests (ctest -R)")
+
+# if testing is enabled download and configure gtest
+list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/cmake/gtest/)
+include(px4_add_gtest)
+if(BUILD_TESTING)
+	include(gtest)
+endif()
+
+add_custom_target(test_results
+		COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test -R ${TESTFILTER} USES_TERMINAL
+		DEPENDS
+			px4
+			examples__dyn_hello
+			test_mixer_multirotor
+		USES_TERMINAL
+		COMMENT "Running tests"
+		WORKING_DIRECTORY ${PX4_BINARY_DIR})
+set_target_properties(test_results PROPERTIES EXCLUDE_FROM_ALL TRUE)
+
 #=============================================================================
 # subdirectories
 #
diff --git a/Makefile b/Makefile
index c1f0202204a6..0c378cffe04e 100644
--- a/Makefile
+++ b/Makefile
@@ -56,9 +56,12 @@ endif
 # directory build/px4_fmu-v2_default and then call make
 # in that directory with the target upload.
 
-#  explicity set default build target
+# explicity set default build target
 all: px4_sitl_default
 
+# define a space character to be able to explicitly find it in strings
+space := $(subst ,, )
+
 # Parsing
 # --------------------------------------------------------------------
 # assume 1st argument passed is the main target, the
@@ -165,8 +168,8 @@ define cmake-cache-check
 	@$(eval CACHED_CMAKE_OPTIONS = $(shell cd $(BUILD_DIR) 2>/dev/null && cmake -L 2>/dev/null | sed -n 's/\([^[:blank:]]*\):[^[:blank:]]*\(=[^[:blank:]]*\)/\1\2/gp' ))
 	@# transform the options in CMAKE_ARGS into the OPTION=VALUE format without -D
 	@$(eval DESIRED_CMAKE_OPTIONS = $(shell echo $(CMAKE_ARGS) | sed -n 's/-D\([^[:blank:]]*=[^[:blank:]]*\)/\1/gp' ))
-	@# find each currently desired option in the already cached ones
-	@$(eval VERIFIED_CMAKE_OPTIONS = $(foreach option,$(DESIRED_CMAKE_OPTIONS),$(findstring $(option),$(CACHED_CMAKE_OPTIONS))))
+	@# find each currently desired option in the already cached ones making sure the complete configured string value is the same
+	@$(eval VERIFIED_CMAKE_OPTIONS = $(foreach option,$(DESIRED_CMAKE_OPTIONS),$(strip $(findstring $(option)$(space),$(CACHED_CMAKE_OPTIONS)))))
 	@# if the complete list of desired options is found in the list of verified options we don't need to reconfigure and CMAKE_CACHE_CHECK stays empty
 	@$(eval CMAKE_CACHE_CHECK = $(if $(findstring $(DESIRED_CMAKE_OPTIONS),$(VERIFIED_CMAKE_OPTIONS)),,y))
 endef
@@ -341,9 +344,13 @@ format:
 .PHONY: rostest python_coverage
 
 tests:
-	@$(MAKE) --no-print-directory px4_sitl_test test_results \
-	ASAN_OPTIONS="color=always:check_initialization_order=1:detect_stack_use_after_return=1" \
-	UBSAN_OPTIONS="color=always"
+	$(eval CMAKE_ARGS += -DCMAKE_TESTING=ON)
+	$(eval CMAKE_ARGS += -DCONFIG=px4_sitl_test)
+	$(eval CMAKE_ARGS += -DTESTFILTER=$(TESTFILTER))
+	$(eval ARGS += test_results)
+	$(eval ASAN_OPTIONS += color=always:check_initialization_order=1:detect_stack_use_after_return=1)
+	$(eval UBSAN_OPTIONS += color=always)
+	$(call cmake-build,px4_sitl_test)
 
 tests_coverage:
 	@$(MAKE) clean
@@ -464,7 +471,6 @@ distclean: gazeboclean
 		$(error "$@ cannot be the first argument. Use '$(MAKE) help|list_config_targets' to get a list of all possible [configuration] targets."),@#)
 
 # Print a list of non-config targets (based on http://stackoverflow.com/a/26339924/1487069)
-space := $(subst ,, )
 help:
 	@echo "Usage: $(MAKE) <target>"
 	@echo "Where <target> is one of:"
diff --git a/cmake/gtest/CMakeLists.txt.in b/cmake/gtest/CMakeLists.txt.in
new file mode 100644
index 000000000000..fb9ff2f0ab99
--- /dev/null
+++ b/cmake/gtest/CMakeLists.txt.in
@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.4)
+
+project(googletest-download NONE)
+
+include(ExternalProject)
+ExternalProject_Add(googletest
+	URL https://github.com/google/googletest/archive/8b6d3f9c4a774bef3081195d422993323b6bb2e0.zip
+	SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
+	BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
+	CONFIGURE_COMMAND ""
+	BUILD_COMMAND ""
+	INSTALL_COMMAND ""
+	TEST_COMMAND ""
+	# Wrap download, configure and build steps in a script to log output
+    LOG_DOWNLOAD ON
+    LOG_CONFIGURE ON
+    LOG_BUILD ON
+)
diff --git a/cmake/gtest/gtest.cmake b/cmake/gtest/gtest.cmake
new file mode 100644
index 000000000000..3b74cfb211be
--- /dev/null
+++ b/cmake/gtest/gtest.cmake
@@ -0,0 +1,49 @@
+############################################################################
+#
+# Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+# Download and unpack googletest at configure time
+configure_file(${CMAKE_CURRENT_LIST_DIR}/CMakeLists.txt.in googletest-download/CMakeLists.txt)
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . RESULT_VARIABLE result1 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download)
+execute_process(COMMAND ${CMAKE_COMMAND} --build . RESULT_VARIABLE result2 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download)
+if(result1 OR result2)
+	message(FATAL_ERROR "Preparing googletest failed: ${result1} ${result2}")
+endif()
+
+# Add googletest, defines gtest and gtest_main targets
+add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src ${CMAKE_CURRENT_BINARY_DIR}/googletest-build EXCLUDE_FROM_ALL)
+
+# Remove visibility.h from the compile flags for gtest because of poisoned exit()
+get_target_property(GTEST_COMPILE_FLAGS gtest COMPILE_OPTIONS)
+list(REMOVE_ITEM GTEST_COMPILE_FLAGS "-include")
+list(REMOVE_ITEM GTEST_COMPILE_FLAGS "visibility.h")
+set_target_properties(gtest PROPERTIES COMPILE_OPTIONS "${GTEST_COMPILE_FLAGS}")
diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake
new file mode 100644
index 000000000000..4ae03387db20
--- /dev/null
+++ b/cmake/gtest/px4_add_gtest.cmake
@@ -0,0 +1,70 @@
+############################################################################
+#
+# Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include(px4_base)
+
+#=============================================================================
+#
+#	px4_add_gtest
+#
+#	Adds a googletest unit test to the test_results target.
+#
+function(px4_add_gtest)
+	# skip if unit testing is not configured
+	if(BUILD_TESTING)
+		# parse source file and library dependencies from arguments
+		px4_parse_function_args(
+			NAME px4_add_gtest
+			ONE_VALUE SRC
+			MULTI_VALUE LINKLIBS
+			REQUIRED SRC
+			ARGN ${ARGN})
+
+		# infer test name from source filname
+		get_filename_component(TESTNAME ${SRC} NAME_WE)
+		string(REPLACE Test "" TESTNAME ${TESTNAME})
+		set(TESTNAME unit-${TESTNAME})
+
+		# build a binary for the unit test
+		add_executable(${TESTNAME} EXCLUDE_FROM_ALL ${SRC})
+
+		# link the libary to test and gtest
+		target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_main)
+
+		# add the test to the ctest plan
+		add_test(NAME ${TESTNAME} COMMAND ${TESTNAME})
+
+		# attach it to the unit test target
+		add_dependencies(test_results ${TESTNAME})
+	endif()
+endfunction()
diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake
index 87aa14895c3b..dcba66cb863a 100644
--- a/platforms/posix/cmake/sitl_tests.cmake
+++ b/platforms/posix/cmake/sitl_tests.cmake
@@ -54,9 +54,10 @@ if (CMAKE_SYSTEM_NAME STREQUAL "CYGWIN")
 endif()
 
 foreach(test_name ${tests})
+	set(test_name_prefix sitl-${test_name})
 	configure_file(${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_template.in ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_${test_name}_generated)
 
-	add_test(NAME ${test_name}
+	add_test(NAME ${test_name_prefix}
 		COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
 			$<TARGET_FILE:px4>
 			none
@@ -66,10 +67,10 @@ foreach(test_name ${tests})
 			${PX4_BINARY_DIR}
 		WORKING_DIRECTORY ${SITL_WORKING_DIR})
 
-	set_tests_properties(${test_name} PROPERTIES FAIL_REGULAR_EXPRESSION "${test_name} FAILED")
-	set_tests_properties(${test_name} PROPERTIES PASS_REGULAR_EXPRESSION "${test_name} PASSED")
+	set_tests_properties(${test_name_prefix} PROPERTIES FAIL_REGULAR_EXPRESSION "${test_name} FAILED")
+	set_tests_properties(${test_name_prefix} PROPERTIES PASS_REGULAR_EXPRESSION "${test_name} PASSED")
 
-	sanitizer_fail_test_on_error(${test_name})
+	sanitizer_fail_test_on_error(${test_name_prefix})
 endforeach()
 
 
@@ -144,18 +145,6 @@ foreach(cmd_name ${test_cmds})
 	set_tests_properties(posix_${cmd_name} PROPERTIES PASS_REGULAR_EXPRESSION "Shutting down")
 endforeach()
 
-
-add_custom_target(test_results
-		COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test
-		DEPENDS
-			px4
-			examples__dyn_hello
-			test_mixer_multirotor
-		USES_TERMINAL
-		COMMENT "Running tests in sitl"
-		WORKING_DIRECTORY ${PX4_BINARY_DIR})
-set_target_properties(test_results PROPERTIES EXCLUDE_FROM_ALL TRUE)
-
 if (CMAKE_BUILD_TYPE STREQUAL Coverage)
 	setup_target_for_coverage(test_coverage "${CMAKE_CTEST_COMMAND} --output-on-failure -T Test" tests)
 	setup_target_for_coverage(generate_coverage "${CMAKE_COMMAND} -E echo" generic)
diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp
new file mode 100644
index 000000000000..ad07cb9d866f
--- /dev/null
+++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <AttitudeControl.hpp>
+
+using namespace matrix;
+
+TEST(AttitudeControlTest, AllZeroCase)
+{
+	AttitudeControl attitude_control;
+	matrix::Vector3f rate_setpoint = attitude_control.update(Quatf(), Quatf(), 0.f);
+	EXPECT_EQ(rate_setpoint, Vector3f());
+}
diff --git a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
index 6e29d4762ac0..f4b1ace74ac9 100644
--- a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
+++ b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
@@ -38,3 +38,5 @@ target_include_directories(AttitudeControl
 	PUBLIC
 	${CMAKE_CURRENT_SOURCE_DIR}
 )
+
+px4_add_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl)