diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index 06e6d86225..a391052af3 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -34,6 +34,7 @@
 set(srcs
 	test_adc.c
 	test_autodeclination.cpp
+	test_hysteresis.cpp
 	test_bson.c
 	test_conv.cpp
 	test_file.c
@@ -86,4 +87,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/systemcmds/tests/test_hysteresis.cpp b/src/systemcmds/tests/test_hysteresis.cpp
new file mode 100644
index 0000000000..6c6dd956e8
--- /dev/null
+++ b/src/systemcmds/tests/test_hysteresis.cpp
@@ -0,0 +1,196 @@
+#include <unit_test/unit_test.h>
+
+#include <systemlib/hysteresis/hysteresis.h>
+
+class HysteresisTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool _init_false();
+	bool _init_true();
+	bool _zero_case();
+	bool _change_after_time();
+	bool _hysteresis_changed();
+	bool _change_after_multiple_sets();
+	bool _take_change_back();
+};
+
+bool HysteresisTest::run_tests()
+{
+	ut_run_test(_init_false);
+	ut_run_test(_init_true);
+	ut_run_test(_zero_case);
+	ut_run_test(_change_after_time);
+	ut_run_test(_hysteresis_changed);
+	ut_run_test(_change_after_multiple_sets);
+	ut_run_test(_take_change_back);
+
+	return (_tests_failed == 0);
+}
+
+bool HysteresisTest::_init_false()
+{
+	systemlib::Hysteresis hysteresis(false);
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_init_true()
+{
+	systemlib::Hysteresis hysteresis(true);
+	ut_assert_true(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_zero_case()
+{
+	// Default is 0 hysteresis.
+	systemlib::Hysteresis hysteresis(false);
+	ut_assert_false(hysteresis.get_state());
+
+	// Change and see result immediately.
+	hysteresis.set_state_and_update(true);
+	ut_assert_true(hysteresis.get_state());
+	hysteresis.set_state_and_update(false);
+	ut_assert_false(hysteresis.get_state());
+	hysteresis.set_state_and_update(true);
+	ut_assert_true(hysteresis.get_state());
+
+	// A wait won't change anything.
+	usleep(1000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_change_after_time()
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+	hysteresis.set_hysteresis_time_from(true, 3000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(4000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+	usleep(2000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+
+	// Change back to false.
+	hysteresis.set_state_and_update(false);
+	ut_assert_true(hysteresis.get_state());
+	usleep(1000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_hysteresis_changed()
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(true, 2000);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+
+	// Change hysteresis time.
+	hysteresis.set_hysteresis_time_from(true, 10000);
+
+	// Change back to false.
+	hysteresis.set_state_and_update(false);
+	ut_assert_true(hysteresis.get_state());
+	usleep(7000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+	usleep(5000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_change_after_multiple_sets()
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(true, 5000);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(true);
+	ut_assert_true(hysteresis.get_state());
+
+	// Change to false.
+	hysteresis.set_state_and_update(false);
+	ut_assert_true(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(false);
+	ut_assert_true(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(false);
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_take_change_back()
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+	// Change your mind to false.
+	hysteresis.set_state_and_update(false);
+	ut_assert_false(hysteresis.get_state());
+	usleep(6000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+
+	// And true again
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+
+	// The other directory is immediate.
+	hysteresis.set_state_and_update(false);
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+ut_declare_test_c(test_hysteresis, HysteresisTest)
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index 2f5004c36c..80e5d27d6d 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -74,6 +74,7 @@ __BEGIN_DECLS
 
 extern int	test_adc(int argc, char *argv[]);
 extern int	test_autodeclination(int argc, char *argv[]);
+extern int	test_hysteresis(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index a733c4eada..9e17214df0 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -97,6 +97,7 @@ const struct {
 	{"uorb",		uorb_tests_main,	0},
 
 	{"autodeclination",		test_autodeclination,	0},
+	{"hysteresis",		test_hysteresis,	0},
 	{"bson",		test_bson,	0},
 	{"conv",		test_conv, 0},
 	{"file",		test_file,	OPT_NOJIGTEST | OPT_NOALLTEST},
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index c6bf0b90be..9df3adcce3 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -1,9 +1,5 @@
 cmake_minimum_required(VERSION 2.8)
 
-include(CMakeForceCompiler)
-#CMAKE_FORCE_C_COMPILER(clang Clang)
-#CMAKE_FORCE_CXX_COMPILER(clang++ Clang)
-
 if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
 	add_compile_options(-Qunused-arguments )
 endif()
@@ -14,28 +10,16 @@ endif()
 project(unittests)
 enable_testing()
 
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=c++11 -g -fno-exceptions -fno-rtti -fno-threadsafe-statics")
 
-if(COMPILER_SUPPORTS_CXX11)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
-elseif(COMPILER_SUPPORTS_CXX0X)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
-else()
-	message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-endif()
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g")
-
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -fsanitize=address -fno-omit-frame-pointer")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-sign-compare -Wno-unused-but-set-variable")
 
 set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
 add_subdirectory(${GTEST_DIR})
 include_directories(${GTEST_DIR}/include)
 
 set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src)
-set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_test)
+set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_default)
 
 include_directories(${CMAKE_SOURCE_DIR})
 include_directories(${PX4_SITL_BUILD}/src)
@@ -52,43 +36,32 @@ include_directories(${PX4_SRC}/modules/uORB)
 include_directories(${PX4_SRC}/platforms)
 include_directories(${PX4_SRC}/platforms/posix/include)
 include_directories(${PX4_SRC}/platforms/posix/px4_layer)
+include_directories(${PX4_SRC}/platforms/posix/work_queue)
 
+add_definitions(-D__CUSTOM_FILE_IO__)
 add_definitions(-D__EXPORT=)
 add_definitions(-D__PX4_POSIX)
 add_definitions(-D__PX4_TESTS)
+add_definitions(-D__PX4_UNIT_TESTS)
 add_definitions(-D_UNIT_TEST=)
 add_definitions(-DERROR=-1)
 add_definitions(-Dmain_t=int)
 add_definitions(-Dnoreturn_function=)
 add_definitions(-DOK=0)
 
-# check
-add_custom_target(check
-		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
-		WORKING_DIR ${CMAKE_BINARY_DIR}
-		USES_TERMINAL)
+if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+	add_definitions(-D__PX4_DARWIN)
+else()
+	add_definitions(-D__PX4_LINUX)
+endif()
 
-function(add_gtest)
-	foreach(test_name ${ARGN})
-		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
-			target_link_libraries(${test_name} gtest_main pthread px4_platform)
-			add_definitions(-D__PX4_DARWIN)
-		else()
-			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
-			add_definitions(-D__PX4_LINUX)
-		endif()
-		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
-		add_dependencies(check ${test_name})
-	endforeach()
-endfunction()
-
-add_library(px4_platform
+add_library(px4_platform 
 			${PX4_SITL_BUILD}/src/modules/param/px4_parameters.c
-			${PX4_SRC}/drivers/device/device_posix.cpp
-			${PX4_SRC}/drivers/device/i2c_posix.cpp
+			${PX4_SRC}/drivers/device/device_posix.cpp 
+			${PX4_SRC}/drivers/device/i2c_posix.cpp 
 			${PX4_SRC}/drivers/device/ringbuffer.cpp
-			${PX4_SRC}/drivers/device/sim.cpp
-			${PX4_SRC}/drivers/device/vdev.cpp
+			${PX4_SRC}/drivers/device/sim.cpp 
+			${PX4_SRC}/drivers/device/vdev.cpp 
 			${PX4_SRC}/drivers/device/vdev_posix.cpp
 			${PX4_SRC}/drivers/device/vfile.cpp
 			${PX4_SRC}/platforms/posix/px4_layer/drv_hrt.c
@@ -114,6 +87,28 @@ add_library(px4_platform
 			${PX4_SRC}/platforms/posix/work_queue/work_thread.c
 			)
 target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
+target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/include)
+target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/work_queue)
+
+
+# check
+add_custom_target(check
+		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
+		WORKING_DIR ${CMAKE_BINARY_DIR}
+		USES_TERMINAL)
+
+# add_gtest
+function(add_gtest)
+	foreach(test_name ${ARGN})
+		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+			target_link_libraries(${test_name} gtest_main pthread px4_platform)
+		else()
+			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
+		endif()
+		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
+		add_dependencies(check ${test_name})
+	endforeach()
+endfunction()
 
 
 #######################################################################
@@ -122,64 +117,9 @@ target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
 #    add_executable(example_test example_test.cpp)
 #    add_gtest(example_test)
 
-
-# autodeclination_test
-add_executable(autodeclination_test autodeclination_test.cpp ${PX4_SRC}/lib/geo_lookup/geo_mag_declination.c)
-add_gtest(autodeclination_test)
-
-# mixer_test
-add_custom_command(OUTPUT ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
-                   COMMAND ${PX4_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
-add_executable(mixer_test mixer_test.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_group.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_load.c
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_simple.cpp
-                          ${PX4_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
-                          ${PX4_SRC}/systemcmds/tests/test_mixer.cpp)
-add_gtest(mixer_test)
-
-# conversion_test
-add_executable(conversion_test conversion_test.cpp ${PX4_SRC}/systemcmds/tests/test_conv.cpp)
-add_gtest(conversion_test)
-
-# sbus2_test
-add_executable(sbus2_test sbus2_test.cpp
-						${PX4_SRC}/lib/rc/sbus.c)
-add_gtest(sbus2_test)
-
-# DSM test
-add_executable(dsm_test dsm_test.cpp
-						${PX4_SRC}/lib/rc/dsm.c)
-add_gtest(dsm_test)
-
-# st24_test
-add_executable(rc_input_test st24_test.cpp sumd_test.cpp
-						${PX4_SRC}/lib/rc/st24.c
-						${PX4_SRC}/lib/rc/sumd.c)
-add_gtest(rc_input_test)
-
-# sf0x_test
-add_executable(sf0x_test sf0x_test.cpp
-						${PX4_SRC}/drivers/sf0x/sf0x_parser.cpp)
-add_gtest(sf0x_test)
-
 # param_test
 add_executable(param_test param_test.cpp uorb_stub.cpp
 						${PX4_SRC}/modules/systemlib/bson/tinybson.c
 						${PX4_SRC}/modules/systemlib/param/param.c)
 target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a)
 add_gtest(param_test)
-
-# hysteresis_test
-add_executable(hysteresis_test hysteresis_test.cpp
-						${PX4_SRC}/modules/systemlib/hysteresis/hysteresis.cpp)
-add_gtest(hysteresis_test)
-
-# param_shmem_test
-#add_executable(param_shmem_test param_test.cpp uorb_stub.cpp
-#						${PX4_SRC}/modules/systemlib/bson/tinybson.c
-#						${PX4_SRC}/modules/systemlib/param/param_shmem.c)
-#add_gtest(param_shmem_test)
diff --git a/unittests/hysteresis_test.cpp b/unittests/hysteresis_test.cpp
deleted file mode 100644
index 99be56f317..0000000000
--- a/unittests/hysteresis_test.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-#include <systemlib/hysteresis/hysteresis.h>
-
-#include "gtest/gtest.h"
-
-
-TEST(HysteresisTest, InitFalse)
-{
-	systemlib::Hysteresis hysteresis(false);
-	ASSERT_FALSE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, InitTrue)
-{
-	systemlib::Hysteresis hysteresis(true);
-	ASSERT_TRUE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, ZeroCase)
-{
-	// Default is 0 hysteresis.
-	systemlib::Hysteresis hysteresis(false);
-	ASSERT_FALSE(hysteresis.get_state());
-
-	// Change and see result immediately.
-	hysteresis.set_state_and_update(true);
-	ASSERT_TRUE(hysteresis.get_state());
-	hysteresis.set_state_and_update(false);
-	ASSERT_FALSE(hysteresis.get_state());
-	hysteresis.set_state_and_update(true);
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// A wait won't change anything.
-	usleep(1000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, ChangeAfterTime)
-{
-	systemlib::Hysteresis hysteresis(false);
-	hysteresis.set_hysteresis_time_from(false, 5000);
-	hysteresis.set_hysteresis_time_from(true, 3000);
-
-	// Change to true.
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(4000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(2000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// Change back to false.
-	hysteresis.set_state_and_update(false);
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(1000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, HysteresisChanged)
-{
-	systemlib::Hysteresis hysteresis(false);
-	hysteresis.set_hysteresis_time_from(true, 2000);
-	hysteresis.set_hysteresis_time_from(false, 5000);
-
-	// Change to true.
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// Change hysteresis time.
-	hysteresis.set_hysteresis_time_from(true, 10000);
-
-	// Change back to false.
-	hysteresis.set_state_and_update(false);
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(7000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(5000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, ChangeAfterTimeMultipleSets)
-{
-	systemlib::Hysteresis hysteresis(false);
-	hysteresis.set_hysteresis_time_from(true, 5000);
-	hysteresis.set_hysteresis_time_from(false, 5000);
-
-	// Change to true.
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.set_state_and_update(true);
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// Change to false.
-	hysteresis.set_state_and_update(false);
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.set_state_and_update(false);
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.set_state_and_update(false);
-	ASSERT_FALSE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, TakeChangeBack)
-{
-	systemlib::Hysteresis hysteresis(false);
-	hysteresis.set_hysteresis_time_from(false, 5000);
-
-	// Change to true.
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-	// Change your mind to false.
-	hysteresis.set_state_and_update(false);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(6000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-
-	// And true again
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// The other directory is immediate.
-	hysteresis.set_state_and_update(false);
-	ASSERT_FALSE(hysteresis.get_state());
-}