Testing cleanup from Daniel Agar

2026-07-15 07:50:36 +08:00 · 2016-07-29 13:27:58 +02:00
parent 99aa5f49fc
commit 6ab9dc0acf
56 changed files with 1930 additions and 3416 deletions
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__sf0x__sf0x_tests
+	MAIN sf0x_tests
+	COMPILE_FLAGS
+	SRCS
+		SF0XTest.cpp
+		../sf0x_parser.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
@@ -0,0 +1,91 @@
+#include <unit_test/unit_test.h>
+
+#include <drivers/sf0x/sf0x_parser.h>
+
+#include <systemlib/err.h>
+
+#include <stdio.h>
+#include <unistd.h>
+
+extern "C" __EXPORT int sf0x_tests_main(int argc, char *argv[]);
+
+class SF0XTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool sf0xTest();
+};
+
+bool SF0XTest::run_tests(void)
+{
+	ut_run_test(sf0xTest);
+
+	return (_tests_failed == 0);
+}
+
+bool SF0XTest::sf0xTest(void)
+{
+	const char _LINE_MAX = 20;
+	//char _linebuf[_LINE_MAX];
+	//_linebuf[0] = '\0';
+
+	const char *lines[] = {"0.01\r\n",
+			       "0.02\r\n",
+			       "0.03\r\n",
+			       "0.04\r\n",
+			       "0",
+			       ".",
+			       "0",
+			       "5",
+			       "\r",
+			       "\n",
+			       "0",
+			       "3\r",
+			       "\n"
+			       "\r\n",
+			       "0.06",
+			       "\r\n"
+			      };
+
+	enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
+	float dist_m;
+	char _parserbuf[_LINE_MAX];
+	unsigned _parsebuf_index = 0;
+
+	for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
+		//printf("\n%s", _linebuf);
+
+		int parse_ret;
+
+		for (int i = 0; i < strlen(lines[l]); i++) {
+			parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
+
+			if (parse_ret == 0) {
+				if (l == 0) {
+					ut_test(dist_m - 0.010000f < 0.001f);
+
+				} else if (l == 1) {
+					ut_test(dist_m - 0.020000f < 0.001f);
+
+				} else if (l == 2) {
+					ut_test(dist_m - 0.030000f < 0.001f);
+
+				} else if (l == 3) {
+					ut_test(dist_m - 0.040000f < 0.001f);
+				}
+
+				//printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
+			}
+		}
+
+		//printf("%s", lines[l]);
+
+	}
+
+	return true;
+}
+
+ut_declare_test_c(sf0x_tests_main, SF0XTest)
+
@@ -34,6 +34,7 @@ px4_add_module(
 	MODULE lib__rc
 	COMPILE_FLAGS
 		-Os
+		-Wno-unused-result
 	SRCS
 		st24.c
 		sumd.c
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+px4_add_module(
+	MODULE lib__rc__rc_tests
+	MAIN rc_tests
+	COMPILE_FLAGS
+		-Wno-unused-result
+	SRCS
+		RCTest.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
@@ -0,0 +1,306 @@
+#include <unit_test/unit_test.h>
+
+#include <systemlib/err.h>
+
+#include <stdio.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+
+#define DSM_DEBUG
+#include <lib/rc/sbus.h>
+#include <lib/rc/dsm.h>
+#include <lib/rc/st24.h>
+#include <lib/rc/sumd.h>
+
+#if !defined(CONFIG_ARCH_BOARD_SITL)
+#define TEST_DATA_PATH "/fs/microsd"
+#else
+#define TEST_DATA_PATH "../../../../src/lib/rc/rc_tests/test_data/"
+#endif
+
+extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]);
+
+class RCTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool dsmTest();
+	bool sbus2Test();
+	bool st24Test();
+	bool sumdTest();
+};
+
+bool RCTest::run_tests(void)
+{
+	ut_run_test(dsmTest);
+	ut_run_test(sbus2Test);
+	ut_run_test(st24Test);
+	ut_run_test(sumdTest);
+
+	return (_tests_failed == 0);
+}
+
+bool RCTest::dsmTest(void)
+{
+
+	const char *filepath = TEST_DATA_PATH "dsm_x_data.txt";
+
+	FILE *fp;
+	fp = fopen(filepath, "rt");
+
+	ut_test(fp != nullptr);
+	//warnx("loading data from: %s", filepath);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	// Init the parser
+	uint8_t frame[20];
+	uint16_t rc_values[18];
+	uint16_t num_values;
+	bool dsm_11_bit;
+	unsigned dsm_frame_drops = 0;
+	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
+
+	int rate_limiter = 0;
+	unsigned last_drop = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+
+		ut_test(ret > 0);
+
+		frame[0] = x;
+		unsigned len = 1;
+
+		// Pipe the data into the parser
+		bool result = dsm_parse(f * 1e6f, &frame[0], len, rc_values, &num_values,
+					&dsm_11_bit, &dsm_frame_drops, max_channels);
+
+		if (result) {
+			//warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
+
+			for (unsigned i = 0; i < num_values; i++) {
+				//printf("chan #%u:\t%d\n", i, (int)rc_values[i]);
+			}
+		}
+
+		if (last_drop != (dsm_frame_drops)) {
+			//warnx("frame dropped, now #%d", (dsm_frame_drops));
+			last_drop = dsm_frame_drops;
+		}
+
+		rate_limiter++;
+	}
+
+	ut_test(ret == EOF);
+
+	return true;
+}
+
+bool RCTest::sbus2Test(void)
+{
+	const char *filepath = TEST_DATA_PATH "sbus2_r7008SB.txt";
+
+	FILE *fp;
+	fp = fopen(filepath, "rt");
+
+	ut_test(fp != nullptr);
+	//warnx("loading data from: %s", filepath);
+
+	// if (argc < 2)
+	// 	errx(1, "Need a filename for the input file");
+
+	//int byte_offset = 7;
+
+	// if (argc > 2) {
+	// 	char* end;
+	// 	byte_offset = strtol(argv[2],&end,10);
+	// }
+
+	//warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	// Init the parser
+	uint8_t frame[SBUS_BUFFER_SIZE];
+	uint16_t rc_values[18];
+	uint16_t num_values;
+	unsigned sbus_frame_drops = 0;
+	unsigned sbus_frame_resets = 0;
+	bool sbus_failsafe;
+	bool sbus_frame_drop;
+	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
+
+	int rate_limiter = 0;
+	unsigned last_drop = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+
+		ut_test(ret > 0);
+
+		frame[0] = x;
+		unsigned len = 1;
+
+		// Pipe the data into the parser
+		hrt_abstime now = hrt_absolute_time();
+
+		// if (rate_limiter % byte_offset == 0) {
+		bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values,
+					 &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels);
+
+		if (result) {
+			//warnx("decoded packet");
+		}
+
+		// }
+
+		if (last_drop != (sbus_frame_drops + sbus_frame_resets)) {
+			warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets));
+			last_drop = sbus_frame_drops + sbus_frame_resets;
+		}
+
+		rate_limiter++;
+	}
+
+	ut_test(ret == EOF);
+
+	return true;
+}
+
+bool RCTest::st24Test(void)
+{
+	const char *filepath = TEST_DATA_PATH "st24_data.txt";
+
+	//warnx("loading data from: %s", filepath);
+
+	FILE *fp;
+
+	fp = fopen(filepath, "rt");
+	ut_test(fp != nullptr);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		ut_test(ret > 0);
+
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			// warnx("FRAME RESET\n\n");
+		}
+
+		uint8_t b = static_cast<uint8_t>(x);
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		//hrt_abstime now = hrt_absolute_time();
+
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		uint16_t channels[20];
+
+		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
+			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+
+			for (unsigned i = 0; i < channel_count; i++) {
+				//int16_t val = channels[i];
+				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+			}
+		}
+	}
+
+	ut_test(ret == EOF);
+
+	return true;
+}
+
+bool RCTest::sumdTest(void)
+{
+	const char *filepath = TEST_DATA_PATH "sumd_data.txt";
+
+	//warnx("loading data from: %s", filepath);
+
+	FILE *fp;
+
+	fp = fopen(filepath, "rt");
+	ut_test(fp != nullptr);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		ut_test(ret > 0);
+
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			// warnx("FRAME RESET\n\n");
+		}
+
+		uint8_t b = static_cast<uint8_t>(x);
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		//hrt_abstime now = hrt_absolute_time();
+
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		uint16_t channels[32];
+
+
+		if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) {
+			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+
+			for (unsigned i = 0; i < channel_count; i++) {
+				//int16_t val = channels[i];
+				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+			}
+		}
+	}
+
+	ut_test(ret == EOF);
+
+	return true;
+}
+
+
+
+ut_declare_test_c(rc_tests_main, RCTest)
+
@@ -50,7 +50,7 @@ class MavlinkFtpTest;
 class MavlinkFTP : public MavlinkStream
 {
 public:
-	/// @brief Contructor is only public so unit test code can new objects.
+	/// @brief Constructor is only public so unit test code can new objects.
 	MavlinkFTP(Mavlink *mavlink);
 	~MavlinkFTP();

@@ -236,7 +236,7 @@ bool MavlinkFtpTest::_list_test(void)
 	return true;
 }

-/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that
+/// @brief Tests for correct response to a List command on a valid directory, but with an offset that
 /// is beyond the last directory entry.
 bool MavlinkFtpTest::_list_eof_test(void)
 {
@@ -263,7 +263,7 @@ bool MavlinkFtpTest::_list_eof_test(void)
 	return true;
 }

-/// @brief Tests for correct reponse to an Open command on a file which does not exist.
+/// @brief Tests for correct response to an Open command on a file which does not exist.
 bool MavlinkFtpTest::_open_badfile_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
@@ -130,7 +130,7 @@ int uORBTest::UnitTest::pubsublatency_main(void)

 	pubsubtest_passed = true;

-	if (static_cast<float>(latency_integral / maxruns) > 40.0f) {
+	if (static_cast<float>(latency_integral / maxruns) > 80.0f) {
 		pubsubtest_res = uORB::ERROR;

 	} else {
@@ -37,6 +37,17 @@

 #include <systemlib/err.h>

+#define ut_declare_test_c(test_function, test_class)	\
+	extern "C" {										\
+		int test_function(int argc, char *argv[])		\
+		{												\
+			test_class* test = new test_class();		\
+			bool success = test->run_tests();			\
+			test->print_results();						\
+			return success ? 0 : -1;					\
+		}												\
+	}
+
 /// @brief Base class to be used for unit tests.
 class __EXPORT UnitTest
 {
@@ -92,6 +103,9 @@ protected:
 		}								\
 	} while (0)

+/// @brief Used to assert a value within a unit test.
+#define ut_test(test) ut_assert("test", test)
+
 /// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert
 /// since it will give you better error reporting of the actual values being compared.
 #define ut_compare(message, v1, v2)								\
@@ -33,14 +33,26 @@

 set(srcs
 	test_adc.c
+	test_autodeclination.cpp
 	test_bson.c
-	test_float.c
+	test_conv.cpp
+	test_file.c
+	test_file2.c
+	test_float.cpp
 	test_gpio.c
 	test_hott_telemetry.c
 	test_hrt.c
-	test_int.c
+	test_int.cpp
 	test_jig_voltages.c
 	test_led.c
+	test_mathlib.cpp
+	test_matrix.cpp
+	test_mixer.cpp
+	test_mount.c
+	test_params.c
+	test_perf.c
+	test_ppm_loopback.c
+	test_rc.c
 	test_sensors.c
 	test_servo.c
 	test_sleep.c
@@ -48,16 +60,7 @@ set(srcs
 	test_uart_console.c
 	test_uart_loopback.c
 	test_uart_send.c
-	test_mixer.cpp
-	test_mathlib.cpp
-	test_file.c
-	test_file2.c
 	tests_main.c
-	test_params.c
-	test_ppm_loopback.c
-	test_rc.c
-	test_conv.cpp
-	test_mount.c
 	)

 if(${OS} STREQUAL "nuttx")
@@ -69,11 +72,15 @@ endif()
 px4_add_module(
 	MODULE systemcmds__tests
 	MAIN tests
-	STACK_MAIN 9000
-	STACK_MAX 9000
+	STACK_MAIN 10000
+	STACK_MAX 10000
 	COMPILE_FLAGS
+		${MODULE_CFLAGS}
 		-Wno-unused-result
 		-Wno-float-equal
+		-Wno-missing-declarations
+		-Wno-double-promotion
+		-Wno-unknown-warning-option
 		-Os
 	SRCS ${srcs}
 	DEPENDS
@@ -0,0 +1,38 @@
+#include <unit_test/unit_test.h>
+
+#include <drivers/drv_hrt.h>
+#include <geo/geo.h>
+#include <px4iofirmware/px4io.h>
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+class AutoDeclinationTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool autodeclination_check();
+};
+
+bool AutoDeclinationTest::autodeclination_check(void)
+{
+	ut_assert("declination differs more than 1 degree", get_mag_declination(47.0, 8.0) - 0.6f < 0.5f);
+
+	return true;
+}
+
+bool AutoDeclinationTest::run_tests(void)
+{
+	ut_run_test(autodeclination_check);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_autodeclination, AutoDeclinationTest)
@@ -59,7 +59,7 @@

 int test_conv(int argc, char *argv[])
 {
-	PX4_INFO("Testing system conversions");
+	//PX4_INFO("Testing system conversions");

 	for (int i = -10000; i <= 10000; i += 1) {
 		float f = i / 10000.0f;
@@ -72,7 +72,7 @@ int test_conv(int argc, char *argv[])
 		}
 	}

-	PX4_INFO("All conversions clean");
+	//PX4_INFO("All conversions clean");

 	return 0;
 }
@@ -1,564 +0,0 @@
-/****************************************************************************
-*
-*   Copyright (c) 2013-2015 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.
-*
-****************************************************************************/
-
-/**
- * @file test_eigen.cpp
- *
- * Eigen test (based of mathlib test)
- * @author Johan Jansen <jnsn.johan@gmail.com>
- * @author Nuno Marques <n.marques21@hotmail.com>
- */
-
-#include <cmath>
-#include <px4_eigen.h>
-#include <mathlib/mathlib.h>
-#include <float.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <string.h>
-#include <time.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-
-#include "tests.h"
-
-using namespace Eigen;
-
-typedef Matrix<float, 10, 1> Vector10f;
-
-static constexpr size_t OPERATOR_ITERATIONS = 30000;
-
-const float min = -M_PI_F;
-const float max = M_PI_F;
-const float step = M_PI_F / 12;
-const float epsilon_f = 1E-4;
-uint8_t err_num;
-
-#define TEST_OP(_title, _op)															\
-	{																						\
-		const hrt_abstime t0 = hrt_absolute_time();						\
-		for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) {									\
-			_op;																			\
-		}																					\
-		printf("%llu: %s: finished in: %.6fus\n", (unsigned long long)t0, _title, static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
-	}
-
-#define VERIFY_OP(_title, _op, __OP_TEST__)												\
-	{																						\
-		_op;																				\
-		if (!(__OP_TEST__)) {																 \
-			printf(_title ": Failed! (" # __OP_TEST__ ")\n");									   \
-			++err_num;																	\
-		}																					\
-	}
-
-#define TEST_OP_VERIFY(_title, _op, __OP_TEST__)										\
-	VERIFY_OP(_title, _op, __OP_TEST__)												\
-	TEST_OP(_title, _op)
-
-#define EXPECT_QUATERNION(q_exp, q_act, epsilon)	\
-	(fabsf(q_exp.x() - q_act.x()) <= epsilon &&			\
-	 fabsf(q_exp.y() - q_act.y()) <= epsilon && 			\
-	 fabsf(q_exp.z() - q_act.z()) <= epsilon &&			\
-	 fabsf(q_exp.w() - q_act.w()) <= epsilon)
-
-#define EXPECT_NEAR(expected, actual, epsilon)		\
-	(fabsf(expected - actual) <= epsilon)
-
-/**
- * @brief
- *	Prints an Eigen::Matrix to stdout
- **/
-template<typename T>
-void printEigen(const Eigen::MatrixBase<T> &b)
-{
-	for (int i = 0; i < b.rows(); ++i) {
-		printf("(");
-
-		for (int j = 0; j < b.cols(); ++j) {
-			if (j > 0) {
-				printf(",");
-			}
-
-			printf("%.3f", static_cast<double>(b(i, j)));
-		}
-
-		printf(")%s\n", i + 1 < b.rows() ? "," : "");
-	}
-}
-
-// Methods definition
-Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy);
-Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q);
-Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy);
-Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q);
-math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q);
-
-/**
- * @brief
- *	Construct new Eigen::Quaternion from euler angles
- *	Right order is YPR.
- **/
-Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy)
-{
-	return Eigen::Quaternionf(
-		       Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
-		       Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
-		       Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
-	       );
-}
-
-/**
- * @brief
- *	Construct new Eigen::Vector3f of euler angles from quaternion
- *	Right order is YPR.
- **/
-Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q)
-{
-	return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
-}
-
-/**
- * @brief
- *	Construct new Eigen::Matrix3f from euler angles
- **/
-Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy)
-{
-	return quatFromEuler(rpy).toRotationMatrix();
-}
-
-/**
- * @brief
- *	Adjust PX4 math::quaternion to Eigen::Quaternionf
- **/
-Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q)
-{
-	return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]);
-}
-
-/**
- * @brief
- *	Adjust Eigen::Quaternionf to PX4 math::quaternion
- **/
-math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q)
-{
-	return math::Quaternion(q.w(), q.x(), q.y(), q.z());
-}
-
-/**
- * @brief
- *	Testing main routine
- **/
-int test_eigen(int argc, char *argv[])
-{
-	int rc = 0;
-	warnx("Testing Eigen math...");
-
-	{
-		Eigen::Vector2f v;
-		Eigen::Vector2f v1(1.0f, 2.0f);
-		Eigen::Vector2f v2(1.0f, -1.0f);
-		float data[2] = {1.0f, 2.0f};
-		TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3);
-		TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1));
-		TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
-		TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
-		TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
-		VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1));
-		VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1));
-		VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
-		VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
-		TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
-	}
-
-	{
-		Eigen::Vector3f v;
-		Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
-		Eigen::Vector3f v2(1.0f, -1.0f, 2.0f);
-		float data[3] = {1.0f, 2.0f, 3.0f};
-		TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3);
-		TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1));
-		TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data));
-		TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f));
-		TEST_OP("Vector3f = Vector3f", v = v1);
-		TEST_OP("Vector3f + Vector3f", v + v1);
-		TEST_OP("Vector3f - Vector3f", v - v1);
-		TEST_OP("Vector3f += Vector3f", v += v1);
-		TEST_OP("Vector3f -= Vector3f", v -= v1);
-		TEST_OP("Vector3f * float", v1 * 2.0f);
-		TEST_OP("Vector3f / float", v1 / 2.0f);
-		TEST_OP("Vector3f *= float", v1 *= 2.0f);
-		TEST_OP("Vector3f /= float", v1 /= 2.0f);
-		TEST_OP("Vector3f dot Vector3f", v.dot(v1));
-		TEST_OP("Vector3f cross Vector3f", v1.cross(v2));
-		TEST_OP("Vector3f length", v1.norm());
-		TEST_OP("Vector3f length squared", v1.squaredNorm());
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-		// Need pragma here instead of moving variable out of TEST_OP and just reference because
-		// TEST_OP measures performance of vector operations.
-		TEST_OP("Vector<3> element read", volatile float a = v1(0));
-		TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]);
-#pragma GCC diagnostic pop
-		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
-		TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f);
-	}
-
-	{
-		Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f);
-		Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f);
-		Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f);
-		Eigen::Vector4f vres;
-		float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
-		TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3);
-		TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1));
-		TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data));
-		TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f));
-		TEST_OP("Vector<4> = Vector<4>", v = v1);
-		TEST_OP("Vector<4> + Vector<4>", v + v1);
-		TEST_OP("Vector<4> - Vector<4>", v - v1);
-		TEST_OP("Vector<4> += Vector<4>", v += v1);
-		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
-		TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
-	}
-
-	{
-		Vector10f v1;
-		v1.Zero();
-		float data[10];
-		TEST_OP("Constructor Vector<10>()", Vector10f v3);
-		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector10f v3(v1));
-		TEST_OP("Constructor Vector<10>(float[])", Vector10f v3(data));
-	}
-
-	{
-		Eigen::Matrix3f m1;
-		m1.setIdentity();
-		Eigen::Matrix3f m2;
-		m2.setIdentity();
-		Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
-		TEST_OP("Matrix3f * Vector3f", m1 * v1);
-		TEST_OP("Matrix3f + Matrix3f", m1 + m2);
-		TEST_OP("Matrix3f * Matrix3f", m1 * m2);
-	}
-
-	{
-		Eigen::Matrix<float, 10, 10> m1;
-		m1.setIdentity();
-		Eigen::Matrix<float, 10, 10> m2;
-		m2.setIdentity();
-		Vector10f v1;
-		v1.Zero();
-		TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
-		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
-		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
-	}
-
-	{
-		printf("%llu: Nonsymmetric matrix operations test...\n", (unsigned long long)hrt_absolute_time());
-		// test nonsymmetric +, -, +=, -=
-
-		const Eigen::Matrix<float, 2, 3> m1_orig =
-			(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
-			 4, 5, 6).finished();
-
-		Eigen::Matrix<float, 2, 3> m1(m1_orig);
-
-		Eigen::Matrix<float, 2, 3> m2;
-		m2 << 2, 4, 6,
-		8, 10, 12;
-
-		Eigen::Matrix<float, 2, 3> m3;
-		m3 << 3, 6, 9,
-		12, 15, 18;
-
-		if (m1 + m2 != m3) {
-			printf("%llu: Matrix<2, 3> + Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
-			printEigen(m1 + m2);
-			printf("!=\n");
-			printEigen(m3);
-			++err_num;
-			rc = 1;
-		}
-
-		if (m3 - m2 != m1) {
-			printf("%llu: Matrix<2, 3> - Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
-			printEigen(m3 - m2);
-			printf("!=\n");
-			printEigen(m1);
-			++err_num;
-			rc = 1;
-		}
-
-		m1 += m2;
-
-		if (m1 != m3) {
-			printf("%llu: Matrix<2, 3> += Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
-			printEigen(m1);
-			printf("!=\n");
-			printEigen(m3);
-			++err_num;
-			rc = 1;
-		}
-
-		m1 -= m2;
-
-		if (m1 != m1_orig) {
-			printf("%llu: Matrix<2, 3> -= Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
-			printEigen(m1);
-			printf("!=\n");
-			printEigen(m1_orig);
-			++err_num;
-			rc = 1;
-		}
-	}
-
-	/* QUATERNION TESTS */
-	{
-		// Test conversion rotation matrix to quaternion and back
-		// against existing PX4 mathlib
-		math::Matrix<3, 3> R_orig;
-		Eigen::Quaternionf q_true;
-		Eigen::Quaternionf q;
-		Eigen::Matrix3f R;
-
-		printf("%llu: Conversion method: Quaternion transformation methods test...\n", (unsigned long long)hrt_absolute_time());
-		printf("%llu: Conversion method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
-
-		/******************************************** TEST 1 ****************************************************/
-		q_true = {0.0f, 0.0f, 0.0f, 1.0f};
-		math::Quaternion q_px4 = {1.0f, 0.0f, 0.0f, 0.0f};
-		Eigen::Quaternionf q_eigen(eigenqFromPx4q(q_px4));
-
-		if (!EXPECT_QUATERNION(q_true, q_eigen, FLT_EPSILON)) {
-			printf("%llu: Value of: Quaternion1 [1.0, 0.0, 0.0, 0.0]\n", (unsigned long long)hrt_absolute_time());
-			printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w());
-			printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
-			++err_num;
-			rc = 1;
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 2 ****************************************************/
-		q_true = {1.0f, 0.0f, 0.0f, 0.0f};
-		Eigen::Quaternionf q2_eigen = {0.0f, 0.0f, 0.0f, 1.0f};
-		math::Quaternion q2_px4(px4qFromEigenq(q2_eigen));
-		Eigen::Quaternionf q2_eigen_(q2_px4.data[3], q2_px4.data[0], q2_px4.data[1], q2_px4.data[2]);
-
-		if (!EXPECT_QUATERNION(q_true, q2_eigen_, FLT_EPSILON)) {
-			printf("%llu: Value of: Quaternion2 [0.0, 0.0, 0.0, 1.0]\n", (unsigned long long)hrt_absolute_time());
-			printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q2_px4.data[0], q2_px4.data[1], q2_px4.data[2], q2_px4.data[3]);
-			printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
-			++err_num;
-			rc = 1;
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 3 ****************************************************/
-		q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
-		q = {0.9833474432563558f, 0.14357217502739184f, 0.10602051106179561f, 0.0342707985504821f};
-
-		if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
-			printf("%llu: Value of: Quaternion [0.9833, 0.1436, 0.1060, 0.0343]\n", (unsigned long long)hrt_absolute_time());
-			printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
-			printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
-			++err_num;
-			rc = 1;
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 4 ****************************************************/
-		q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
-		q = {0.7222365948153096f, -0.6390766544101811f, -0.2385737751841646f, 0.11418355701173476f};
-
-		if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
-			printf("%llu: Value of: Quaternion [0.7222, -0.6391, -0.2386, 0.1142]\n", (unsigned long long)hrt_absolute_time());
-			printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
-			printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
-			++err_num;
-			rc = 1;
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 5 ****************************************************/
-		q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3));
-		q = {0.6830127018922193f, 0.18301270189221933f, -0.6830127018922193f, 0.18301270189221933f};
-
-		for (size_t i = 0; i < 4; i++) {
-			if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
-				printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n",
-				       (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 6 ****************************************************/
-		printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
-
-		for (float roll = min; roll <= max; roll += step) {
-			for (float pitch = min; pitch <= max; pitch += step) {
-				for (float yaw = min; yaw <= max; yaw += step) {
-
-					q = Eigen::Quaternionf(quatFromEuler(Eigen::Vector3f(roll, pitch, yaw)));
-
-					R = q.toRotationMatrix();
-					R_orig.from_euler(roll, pitch, yaw);
-
-					for (size_t i = 0; i < 3; i++) {
-						for (size_t j = 0; j < 3; j++) {
-							if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) {
-								printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n",
-								       (unsigned long long)hrt_absolute_time(), (int)i, (int)j);
-								printf("Actual:  \t%8.5f\n", R(i, j));
-								printf("Expected: \t%8.5f\n", R_orig(i, j));
-								++err_num;
-								rc = 1;
-							}
-						}
-					}
-				}
-			}
-		}
-	}
-
-	{
-		// Test rotation method (rotate vector by quaternion)
-		Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
-		Eigen::Vector3f vector_q;
-		Eigen::Vector3f vector_r;
-		Eigen::Quaternionf q;
-		Eigen::Matrix3f R;
-
-		printf("%llu: Rotation method: Quaternion vector rotation method test...\n", (unsigned long long)hrt_absolute_time());
-		printf("%llu: Rotation method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
-
-		/******************************************** TEST 1 ****************************************************/
-		q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F));
-		vector_q = q._transformVector(vector);
-		Eigen::Vector3f vector_true = { -1.00f, 1.00f, 1.00f};
-
-		for (size_t i = 0; i < 3; i++) {
-			if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) {
-				printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f]\n", vector_q(0), vector_q(1), vector_q(2));
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", vector_true(0), vector_true(1), vector_true(2));
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 2 ****************************************************/
-		q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
-		vector_q = q._transformVector(vector);
-		vector_true = {0.8795481794122900f, 1.2090975499501229f, 0.874344391414010f};
-
-		for (size_t i = 0; i < 3; i++) {
-			if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
-				printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 3 ****************************************************/
-		q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
-		vector_q = q._transformVector(vector);
-		vector_true = {0.447416342848463f, -0.6805264343934600f, 1.528627615949624f};
-
-		for (size_t i = 0; i < 3; i++) {
-			if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
-				printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 4 ****************************************************/
-		q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
-		vector_q = q._transformVector(vector);
-		vector_true = { -1.366030f, 0.366025f, 1.000000f};
-
-		for (size_t i = 0; i < 3; i++) {
-			if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
-				printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 5 ****************************************************/
-		printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
-
-		Eigen::Vector3f vectorR(1.0f, 1.0f, 1.0f);
-
-		for (float roll = min; roll <= max; roll += step) {
-			for (float pitch = min; pitch <= max; pitch += step) {
-				for (float yaw = min; yaw <= max; yaw += step) {
-
-					R = matrixFromEuler(Eigen::Vector3f(roll, pitch, yaw));
-					q = quatFromEuler(Eigen::Vector3f(roll, pitch, yaw));
-
-					vector_r = R * vectorR;
-					vector_q = q._transformVector(vectorR);
-
-					for (int i = 0; i < 3; i++) {
-						if (!EXPECT_NEAR(vector_r(i), vector_q(i), epsilon_f)) {
-							printf("%llu: (%d) Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), i);
-							printf("Actual:  \t%8.5f\n", vector_q(i));
-							printf("Expected: \t%8.5f\n", vector_r(i));
-							++err_num;
-							rc = 1;
-						}
-					}
-				}
-			}
-		}
-	}
-	printf("%llu: Finished Eigen math tests with %d error(s)...\n", (unsigned long long)hrt_absolute_time(), err_num);
-	return rc;
-}
@@ -1,287 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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.
- *
- ****************************************************************************/
-
-/**
- * @file tests_float.c
- * Floating point tests
- */
-
-#include <px4_config.h>
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include "tests.h"
-#include <math.h>
-#include <float.h>
-
-typedef union {
-	float f;
-	double d;
-	uint8_t b[8];
-} test_float_double_t;
-
-int test_float(int argc, char *argv[])
-{
-	int ret = 0;
-
-	printf("\n--- SINGLE PRECISION TESTS ---\n");
-	printf("The single precision test involves calls to fabsf(),\nif test fails check this function as well.\n\n");
-
-	float f1 = 1.55f;
-
-	float sinf_zero = sinf(0.0f);
-	float sinf_one = sinf(1.0f);
-	float sqrt_two = sqrt(2.0f);
-
-	if (fabsf(sinf_zero) < FLT_EPSILON) {
-		printf("\t success: sinf(0.0f) == 0.0f\n");
-
-	} else {
-		printf("\t FAIL: sinf(0.0f) != 0.0f, result: %8.4f\n", (double)sinf_zero);
-		ret = -4;
-	}
-
-	fflush(stdout);
-
-	if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
-		printf("\t success: sinf(1.0f) == 0.84147f\n");
-
-	} else {
-		printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %8.4f\n", (double)sinf_one);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-	float asinf_one = asinf(1.0f);
-
-	if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
-		printf("\t success: asinf(1.0f) == 1.57079f\n");
-
-	} else {
-		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-	float cosf_one = cosf(1.0f);
-
-	if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
-		printf("\t success: cosf(1.0f) == 0.54030f\n");
-
-	} else {
-		printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %8.4f\n", (double)cosf_one);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-
-	float acosf_one = acosf(1.0f);
-
-	if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
-		printf("\t success: acosf(1.0f) == 0.0f\n");
-
-	} else {
-		printf("\t FAIL: acosf(1.0f) != 0.0f, result: %8.4f\n", (double)acosf_one);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-
-	float sinf_zero_one = sinf(0.1f);
-
-	if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
-		printf("\t success: sinf(0.1f) == 0.09983f\n");
-
-	} else {
-		printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %8.4f\n", (double)sinf_zero_one);
-		ret = -2;
-	}
-
-	if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) {
-		printf("\t success: sqrt(2.0f) == 1.41421f\n");
-
-	} else {
-		printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %8.4f\n", (double)sinf_zero_one);
-		ret = -3;
-	}
-
-	float atan2f_ones = atan2f(1.0f, 1.0f);
-
-	if (fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON) {
-		printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n");
-
-	} else {
-		printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %8.4f\n", (double)atan2f_ones);
-		ret = -4;
-	}
-
-	char sbuf[30];
-	sprintf(sbuf, "%8.4f", (double)0.553415f);
-
-	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
-	    sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
-	    && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
-		printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", (double)0.553415f);
-
-	} else {
-		printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \"  0.5534\", result: %s\n", sbuf);
-		ret = -5;
-	}
-
-	sprintf(sbuf, "%8.4f", (double) - 0.553415f);
-
-	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
-	    sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
-	    && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
-		printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double) - 0.553415f);
-
-	} else {
-		printf("\t FAIL: printf(\"%%8.4f\", -0.553415f) != \" -0.5534\", result: %s\n", sbuf);
-		ret = -6;
-	}
-
-
-
-
-
-	printf("\n--- DOUBLE PRECISION TESTS ---\n");
-
-	double d1 = 1.0111;
-	double d2 = 2.0;
-
-	double d1d2 = d1 * d2;
-
-	if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) {
-		printf("\t success: 1.0111 * 2.0 == 2.0222\n");
-
-	} else {
-		printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %8.4f\n", d1d2);
-		ret = -7;
-	}
-
-	fflush(stdout);
-
-	// Assign value of f1 to d1
-	d1 = f1;
-
-	if (fabsf(f1 - (float)d1) < FLT_EPSILON) {
-		printf("\t success: (float) 1.55f == 1.55 (double)\n");
-
-	} else {
-		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1);
-		ret = -8;
-	}
-
-	fflush(stdout);
-
-
-	double sin_zero = sin(0.0);
-	double sin_one = sin(1.0);
-	double atan2_ones = atan2(1.0, 1.0);
-
-	if (fabs(sin_zero - 0.0) < DBL_EPSILON) {
-		printf("\t success: sin(0.0) == 0.0\n");
-
-	} else {
-		printf("\t FAIL: sin(0.0) != 0.0, result: %8.4f\n", sin_zero);
-		ret = -9;
-	}
-
-	if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) {
-		printf("\t success: sin(1.0) == 0.84147098480\n");
-
-	} else {
-		printf("\t FAIL: sin(1.0) != 1.0, result: %8.4f\n", sin_one);
-		ret = -10;
-	}
-
-	if (fabs(atan2_ones - 0.785398163397448278999490867136) < 2.0 * DBL_EPSILON) {
-		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
-
-	} else {
-		printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %8.4f\n", atan2_ones);
-		ret = -11;
-	}
-
-	printf("\t testing pow() with magic value\n");
-	printf("\t   (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
-	fflush(stdout);
-	usleep(20000);
-	double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
-	printf("\t success: result: %8.4f\n", (double)powres);
-
-	sprintf(sbuf, "%8.4f", 0.553415);
-
-	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
-	    sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
-	    && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
-		printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
-
-	} else {
-		printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \"  0.5534\", result: %s\n", sbuf);
-		ret = -12;
-	}
-
-	sprintf(sbuf, "%8.4f", -0.553415);
-
-	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
-	    sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
-	    && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
-		printf("\t success: printf(\"%%8.4f\", -0.553415) == %8.4f\n", -0.553415);
-
-	} else {
-		printf("\t FAIL: printf(\"%%8.4f\", -0.553415) != \" -0.5534\", result: %s\n", sbuf);
-		ret = -13;
-	}
-
-
-	if (ret == 0) {
-		printf("\n SUCCESS: All float and double tests passed.\n");
-
-	} else {
-		printf("\n FAIL: One or more tests failed.\n");
-	}
-
-	printf("\n");
-
-	return ret;
-}
@@ -0,0 +1,149 @@
+#include <unit_test/unit_test.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <float.h>
+#include <math.h>
+#include <px4_config.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+typedef union {
+	float f;
+	double d;
+	uint8_t b[8];
+} test_float_double_t;
+
+
+
+class FloatTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool singlePrecisionTests();
+	bool doublePrecisionTests();
+};
+
+bool FloatTest::singlePrecisionTests(void)
+{
+	float sinf_zero = sinf(0.0f);
+	float sinf_one = sinf(1.0f);
+	float sqrt_two = sqrt(2.0f);
+
+	ut_assert("sinf(0.0f) == 0.0f", fabsf(sinf_zero) < FLT_EPSILON);
+	ut_assert("sinf(1.0f) == 0.84147f", fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON);
+
+	float asinf_one = asinf(1.0f);
+	ut_assert("asinf(1.0f) == 1.57079f", fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f);
+
+	float cosf_one = cosf(1.0f);
+	ut_assert("cosf(1.0f) == 0.54030f", fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON);
+
+	float acosf_one = acosf(1.0f);
+	ut_assert("acosf(1.0f) == 0.0f", fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON);
+
+	float sinf_zero_one = sinf(0.1f);
+	ut_assert("sinf(0.1f) == 0.09983f", fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON);
+
+	ut_assert("sqrt(2.0f) == 1.41421f", fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON);
+
+	float atan2f_ones = atan2f(1.0f, 1.0f);
+	ut_assert("atan2f(1.0f, 1.0f) == 0.78539f",
+		  fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON);
+
+	char sbuf[30];
+	sprintf(sbuf, "%8.4f", (double)0.553415f);
+	ut_compare("sbuf[0]", sbuf[0], ' ');
+	ut_compare("sbuf[1]", sbuf[1], ' ');
+	ut_compare("sbuf[2]", sbuf[2], '0');
+	ut_compare("sbuf[3]", sbuf[3], '.');
+	ut_compare("sbuf[4]", sbuf[4], '5');
+	ut_compare("sbuf[5]", sbuf[5], '5');
+	ut_compare("sbuf[6]", sbuf[6], '3');
+	ut_compare("sbuf[7]", sbuf[7], '4');
+	ut_compare("sbuf[8]", sbuf[8], '\0');
+
+	sprintf(sbuf, "%8.4f", (double) - 0.553415f);
+	ut_compare("sbuf[0]", sbuf[0], ' ');
+	ut_compare("sbuf[1]", sbuf[1], '-');
+	ut_compare("sbuf[2]", sbuf[2], '0');
+	ut_compare("sbuf[3]", sbuf[3], '.');
+	ut_compare("sbuf[4]", sbuf[4], '5');
+	ut_compare("sbuf[5]", sbuf[5], '5');
+	ut_compare("sbuf[6]", sbuf[6], '3');
+	ut_compare("sbuf[7]", sbuf[7], '4');
+	ut_compare("sbuf[8]", sbuf[8], '\0');
+
+	return true;
+}
+
+
+bool FloatTest::doublePrecisionTests(void)
+{
+	float f1 = 1.55f;
+
+	double d1 = 1.0111;
+	double d2 = 2.0;
+
+	double d1d2 = d1 * d2;
+
+	ut_assert("1.0111 * 2.0 == 2.0222", fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON);
+
+	// Assign value of f1 to d1
+	d1 = f1;
+
+	ut_assert("(float) 1.55f == 1.55 (double)", fabsf(f1 - (float)d1) < FLT_EPSILON);
+
+
+	double sin_zero = sin(0.0);
+	double sin_one = sin(1.0);
+	double atan2_ones = atan2(1.0, 1.0);
+
+	ut_assert("sin(0.0) == 0.0", fabs(sin_zero - 0.0) < DBL_EPSILON);
+	ut_assert("sin(1.0) == 0.84147098480", fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON);
+	ut_assert("atan2(1.0, 1.0) == 0.785398", fabs(atan2_ones - 0.785398163397448278999490867136) < 2.0 * DBL_EPSILON);
+	ut_assert("testing pow() with magic value",
+		  (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295))) - 428.2293 <  DBL_EPSILON);
+
+
+	char sbuf[30];
+	sprintf(sbuf, "%8.4f", 0.553415);
+	ut_compare("sbuf[0]", sbuf[0], ' ');
+	ut_compare("sbuf[1]", sbuf[1], ' ');
+	ut_compare("sbuf[2]", sbuf[2], '0');
+	ut_compare("sbuf[3]", sbuf[3], '.');
+	ut_compare("sbuf[4]", sbuf[4], '5');
+	ut_compare("sbuf[5]", sbuf[5], '5');
+	ut_compare("sbuf[6]", sbuf[6], '3');
+	ut_compare("sbuf[7]", sbuf[7], '4');
+	ut_compare("sbuf[8]", sbuf[8], '\0');
+
+
+	sprintf(sbuf, "%8.4f", -0.553415);
+	ut_compare("sbuf[0]", sbuf[0], ' ');
+	ut_compare("sbuf[1]", sbuf[1], '-');
+	ut_compare("sbuf[2]", sbuf[2], '0');
+	ut_compare("sbuf[3]", sbuf[3], '.');
+	ut_compare("sbuf[4]", sbuf[4], '5');
+	ut_compare("sbuf[5]", sbuf[5], '5');
+	ut_compare("sbuf[6]", sbuf[6], '3');
+	ut_compare("sbuf[7]", sbuf[7], '4');
+	ut_compare("sbuf[8]", sbuf[8], '\0');
+
+	return true;
+}
+
+bool FloatTest::run_tests(void)
+{
+	ut_run_test(singlePrecisionTests);
+	ut_run_test(doublePrecisionTests);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_float, FloatTest)
@@ -1,152 +0,0 @@
-/****************************************************************************
- * px4/sensors/test_gpio.c
- *
- *  Copyright (C) 2012 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 NuttX 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.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <inttypes.h>
-
-#include <px4_config.h>
-#include <px4_defines.h>
-
-#include <sys/types.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-
-#include <arch/board/board.h>
-
-#include "tests.h"
-
-#include <math.h>
-#include <float.h>
-
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
-typedef union {
-	int32_t i;
-	int64_t l;
-	uint8_t b[8];
-} test_32_64_t;
-
-int test_int(int argc, char *argv[])
-{
-	int ret = 0;
-
-	printf("\n--- 64 BIT MATH TESTS ---\n");
-
-	int64_t large = 354156329598;
-
-	int64_t calc = large * 5;
-
-	if (calc == 1770781647990) {
-		printf("\t success: 354156329598 * 5 == %" PRId64 "\n", calc);
-
-	} else {
-		printf("\t FAIL: 354156329598 * 5 != %" PRId64 "\n", calc);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-
-
-
-
-	printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n");
-
-
-	int32_t small = 50;
-	int32_t large_int = 2147483647; // MAX INT value
-
-	uint64_t small_times_large = large_int * (uint64_t)small;
-
-	if (small_times_large == 107374182350) {
-		printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %" PRId64 "\n", small_times_large);
-
-	} else {
-		printf("\t FAIL: 50 * 2147483647 != %" PRId64 ", 64bit cast might fail\n", small_times_large);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-	if (ret == 0) {
-		printf("\n SUCCESS: All float and double tests passed.\n");
-
-	} else {
-		printf("\n FAIL: One or more tests failed.\n");
-	}
-
-	printf("\n");
-
-	return ret;
-}
@@ -0,0 +1,61 @@
+#include <unit_test/unit_test.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <float.h>
+#include <math.h>
+#include <px4_config.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+typedef union {
+	int32_t i;
+	int64_t l;
+	uint8_t b[8];
+} test_32_64_t;
+
+class IntTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool math64bitTests();
+	bool math3264MixedMathTests();
+};
+
+bool IntTest::math64bitTests(void)
+{
+	int64_t large = 354156329598;
+	int64_t calc = large * 5;
+
+	ut_assert("354156329598 * 5 == 1770781647990", calc == 1770781647990);
+
+	return true;
+}
+
+bool IntTest::math3264MixedMathTests(void)
+{
+	int32_t small = 50;
+	int32_t large_int = 2147483647; // MAX INT value
+
+	uint64_t small_times_large = large_int * (uint64_t)small;
+
+	ut_assert("64bit calculation: 50 * 2147483647 (max int val) == 107374182350", small_times_large == 107374182350);
+
+	return true;
+}
+
+
+bool IntTest::run_tests(void)
+{
+	ut_run_test(math64bitTests);
+	ut_run_test(math3264MixedMathTests);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_int, IntTest)
@@ -31,11 +31,18 @@
 *
 ****************************************************************************/

-/**
- * @file test_mathlib.cpp
- *
- * Mathlib test
- */
+#include <unit_test/unit_test.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <float.h>
+#include <math.h>
+#include <px4_config.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>

 #include <px4_log.h>
 #include <stdio.h>
@@ -47,17 +54,33 @@
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>

+class MathlibTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool testVector2();
+	bool testVector3();
+	bool testVector4();
+	bool testVector10();
+	bool testMatrix3x3();
+	bool testMatrix10x10();
+	bool testMatrixNonsymmetric();
+	bool testRotationMatrixQuaternion();
+	bool testQuaternionfrom_dcm();
+	bool testQuaternionfrom_euler();
+	bool testQuaternionRotate();
+};
+
 #include "tests.h"

 #define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); }

 using namespace math;

-int test_mathlib(int argc, char *argv[])
+bool MathlibTest::testVector2(void)
 {
-	int rc = 0;
-	PX4_INFO("testing mathlib");
-
 	{
 		Vector<2> v;
 		Vector<2> v1(1.0f, 2.0f);
@@ -75,6 +98,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Vector<2> * Vector<2>", v * v1);
 		TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
 	}
+	return true;
+}
+
+bool MathlibTest::testVector3(void)
+{

 	{
 		Vector<3> v;
@@ -108,7 +136,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
 		TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
 	}
+	return true;
+}

+bool MathlibTest::testVector4(void)
+{
 	{
 		Vector<4> v;
 		Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
@@ -125,7 +157,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
 		TEST_OP("Vector<4> * Vector<4>", v * v1);
 	}
+	return true;
+}

+bool MathlibTest::testVector10(void)
+{
 	{
 		Vector<10> v1;
 		v1.zero();
@@ -134,7 +170,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
 		TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
 	}
+	return true;
+}

+bool MathlibTest::testMatrix3x3(void)
+{
 	{
 		Matrix<3, 3> m1;
 		m1.identity();
@@ -145,7 +185,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
 		TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
 	}
+	return true;
+}

+bool MathlibTest::testMatrix10x10(void)
+{
 	{
 		Matrix<10, 10> m1;
 		m1.identity();
@@ -157,9 +201,14 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
 		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
 	}
+	return true;
+}

+bool MathlibTest::testMatrixNonsymmetric(void)
+{
+	int rc = true;
 	{
-		PX4_INFO("Nonsymmetric matrix operations test");
+		//PX4_INFO("Nonsymmetric matrix operations test");
 		// test nonsymmetric +, -, +=, -=

 		float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
@@ -175,17 +224,21 @@ int test_mathlib(int argc, char *argv[])
 			(m1 + m2).print();
 			printf("!=\n");
 			m3.print();
-			rc = 1;
+			rc = false;
 		}

+		ut_assert("m1 + m2 == m3", m1 + m2 == m3);
+
 		if (m3 - m2 != m1) {
 			PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!");
 			(m3 - m2).print();
 			printf("!=\n");
 			m1.print();
-			rc = 1;
+			rc = false;
 		}

+		ut_assert("m3 - m2 == m1", m3 - m2 == m1);
+
 		m1 += m2;

 		if (m1 != m3) {
@@ -193,9 +246,11 @@ int test_mathlib(int argc, char *argv[])
 			m1.print();
 			printf("!=\n");
 			m3.print();
-			rc = 1;
+			rc = false;
 		}

+		ut_assert("m1 == m3", m1 == m3);
+
 		m1 -= m2;
 		Matrix<2, 3> m1_orig(data1);

@@ -204,160 +259,181 @@ int test_mathlib(int argc, char *argv[])
 			m1.print();
 			printf("!=\n");
 			m1_orig.print();
-			rc = 1;
+			rc = false;
 		}

-	}
+		ut_assert("m1 == m1_orig", m1 == m1_orig);

-	{
-		// test conversion rotation matrix to quaternion and back
-		math::Matrix<3, 3> R_orig;
-		math::Matrix<3, 3> R;
-		math::Quaternion q;
-		float diff = 0.1f;
-		float tol = 0.00001f;
-
-		PX4_INFO("Quaternion transformation methods test.");
-
-		for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
-			for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
-				for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
-					R_orig.from_euler(roll, pitch, yaw);
-					q.from_dcm(R_orig);
-					R = q.to_dcm();
-
-					for (int i = 0; i < 3; i++) {
-						for (int j = 0; j < 3; j++) {
-							if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) {
-								PX4_WARN("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!");
-								rc = 1;
-							}
-						}
-					}
-				}
-			}
-		}
-
-		// test against some known values
-		tol = 0.0001f;
-		math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
-		R_orig.identity();
-		q.from_dcm(R_orig);
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
-				PX4_WARN("Quaternion method 'from_dcm()' outside tolerance!");
-				rc = 1;
-			}
-		}
-
-		q_true.from_euler(0.3f, 0.2f, 0.1f);
-		q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
-				PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
-				rc = 1;
-			}
-		}
-
-		q_true.from_euler(-1.5f, -0.2f, 0.5f);
-		q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
-				PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
-				rc = 1;
-			}
-		}
-
-		q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
-		q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
-				PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
-				rc = 1;
-			}
-		}
-
-	}
-
-	{
-		// test quaternion method "rotate" (rotate vector by quaternion)
-		Vector<3> vector = {1.0f, 1.0f, 1.0f};
-		Vector<3> vector_q;
-		Vector<3> vector_r;
-		Quaternion q;
-		Matrix<3, 3> R;
-		float diff = 0.1f;
-		float tol = 0.00001f;
-
-		PX4_INFO("Quaternion vector rotation method test.");
-
-		for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
-			for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
-				for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
-					R.from_euler(roll, pitch, yaw);
-					q.from_euler(roll, pitch, yaw);
-					vector_r = R * vector;
-					vector_q = q.conjugate(vector);
-
-					for (int i = 0; i < 3; i++) {
-						if (fabsf(vector_r(i) - vector_q(i)) > tol) {
-							PX4_WARN("Quaternion method 'rotate' outside tolerance");
-							rc = 1;
-						}
-					}
-				}
-			}
-		}
-
-		// test some values calculated with matlab
-		tol = 0.0001f;
-		q.from_euler(M_PI_2_F, 0.0f, 0.0f);
-		vector_q = q.conjugate(vector);
-		Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
-
-		for (unsigned i = 0; i < 3; i++) {
-			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
-				PX4_WARN("Quaternion method 'rotate' outside tolerance");
-				rc = 1;
-			}
-		}
-
-		q.from_euler(0.3f, 0.2f, 0.1f);
-		vector_q = q.conjugate(vector);
-		vector_true = {1.1566, 0.7792, 1.0273};
-
-		for (unsigned i = 0; i < 3; i++) {
-			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
-				PX4_WARN("Quaternion method 'rotate' outside tolerance");
-				rc = 1;
-			}
-		}
-
-		q.from_euler(-1.5f, -0.2f, 0.5f);
-		vector_q = q.conjugate(vector);
-		vector_true = {0.5095, 1.4956, -0.7096};
-
-		for (unsigned i = 0; i < 3; i++) {
-			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
-				PX4_WARN("Quaternion method 'rotate' outside tolerance");
-				rc = 1;
-			}
-		}
-
-		q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
-		vector_q = q.conjugate(vector);
-		vector_true = { -1.3660, 0.3660, 1.0000};
-
-		for (unsigned i = 0; i < 3; i++) {
-			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
-				PX4_WARN("Quaternion method 'rotate' outside tolerance");
-				rc = 1;
-			}
-		}
 	}
 	return rc;
 }
+
+bool MathlibTest::testRotationMatrixQuaternion(void)
+{
+	// test conversion rotation matrix to quaternion and back
+	math::Matrix<3, 3> R_orig;
+	math::Matrix<3, 3> R;
+	math::Quaternion q;
+	float diff = 0.2f;
+	float tol = 0.00001f;
+
+	//PX4_INFO("Quaternion transformation methods test.");
+
+	for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
+		for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
+			for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
+				R_orig.from_euler(roll, pitch, yaw);
+				q.from_dcm(R_orig);
+				R = q.to_dcm();
+
+				for (int i = 0; i < 3; i++) {
+					for (int j = 0; j < 3; j++) {
+						ut_assert("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig.data[i][j] - R.data[i][j]) < tol);
+					}
+				}
+			}
+		}
+	}
+
+	return true;
+}
+
+
+bool MathlibTest::testQuaternionfrom_dcm(void)
+{
+	// test against some known values
+	float tol = 0.0001f;
+	math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
+
+	math::Matrix<3, 3> R_orig;
+	R_orig.identity();
+
+	math::Quaternion q;
+	q.from_dcm(R_orig);
+
+	for (unsigned i = 0; i < 4; i++) {
+		ut_assert("Quaternion method 'from_dcm()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
+	}
+
+	return true;
+}
+
+bool MathlibTest::testQuaternionfrom_euler(void)
+{
+	float tol = 0.0001f;
+	math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
+
+	math::Matrix<3, 3> R_orig;
+	R_orig.identity();
+
+	math::Quaternion q;
+	q.from_dcm(R_orig);
+
+	q_true.from_euler(0.3f, 0.2f, 0.1f);
+	q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
+
+	for (unsigned i = 0; i < 4; i++) {
+		ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
+	}
+
+	q_true.from_euler(-1.5f, -0.2f, 0.5f);
+	q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
+
+	for (unsigned i = 0; i < 4; i++) {
+		ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
+	}
+
+	q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
+	q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
+
+	for (unsigned i = 0; i < 4; i++) {
+		ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
+	}
+
+	return true;
+}
+
+bool MathlibTest::testQuaternionRotate(void)
+{
+	// test quaternion method "rotate" (rotate vector by quaternion)
+	Vector<3> vector = {1.0f, 1.0f, 1.0f};
+	Vector<3> vector_q;
+	Vector<3> vector_r;
+	Quaternion q;
+	Matrix<3, 3> R;
+	float diff = 0.2f;
+	float tol = 0.00001f;
+
+	//PX4_INFO("Quaternion vector rotation method test.");
+
+	for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
+		for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
+			for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
+				R.from_euler(roll, pitch, yaw);
+				q.from_euler(roll, pitch, yaw);
+				vector_r = R * vector;
+				vector_q = q.conjugate(vector);
+
+				for (int i = 0; i < 3; i++) {
+					ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
+				}
+			}
+		}
+	}
+
+
+	// test some values calculated with matlab
+	tol = 0.0001f;
+	q.from_euler(M_PI_2_F, 0.0f, 0.0f);
+	vector_q = q.conjugate(vector);
+	Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
+
+	for (unsigned i = 0; i < 3; i++) {
+		ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
+	}
+
+	q.from_euler(0.3f, 0.2f, 0.1f);
+	vector_q = q.conjugate(vector);
+	vector_true = {1.1566, 0.7792, 1.0273};
+
+	for (unsigned i = 0; i < 3; i++) {
+		ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
+	}
+
+	q.from_euler(-1.5f, -0.2f, 0.5f);
+	vector_q = q.conjugate(vector);
+	vector_true = {0.5095, 1.4956, -0.7096};
+
+	for (unsigned i = 0; i < 3; i++) {
+		ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
+	}
+
+	q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
+	vector_q = q.conjugate(vector);
+	vector_true = { -1.3660, 0.3660, 1.0000};
+
+	for (unsigned i = 0; i < 3; i++) {
+		ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
+	}
+
+	return true;
+}
+
+bool MathlibTest::run_tests(void)
+{
+	ut_run_test(testVector2);
+	ut_run_test(testVector3);
+	ut_run_test(testVector4);
+	ut_run_test(testVector10);
+	ut_run_test(testMatrix3x3);
+	ut_run_test(testMatrix10x10);
+	ut_run_test(testMatrixNonsymmetric);
+	ut_run_test(testRotationMatrixQuaternion);
+	ut_run_test(testQuaternionfrom_dcm);
+	ut_run_test(testQuaternionfrom_euler);
+	ut_run_test(testQuaternionRotate);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_mathlib, MathlibTest)
@@ -0,0 +1,679 @@
+
+#include <unit_test/unit_test.h>
+
+#include <matrix/math.hpp>
+#include <matrix/filter.hpp>
+#include <matrix/integration.hpp>
+
+using namespace matrix;
+
+class MatrixTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool attitudeTests();
+	bool filterTests();
+	bool helperTests();
+	bool integrationTests();
+	bool inverseTests();
+	bool matrixAssignmentTests();
+	bool matrixMultTests();
+	bool matrixScalarMultTests();
+	bool setIdentityTests();
+	bool sliceTests();
+	bool squareMatrixTests();
+	bool transposeTests();
+	bool vectorTests();
+	bool vector2Tests();
+	bool vector3Tests();
+	bool vectorAssignmentTests();
+};
+
+bool MatrixTest::run_tests(void)
+{
+	ut_run_test(attitudeTests);
+	ut_run_test(filterTests);
+	ut_run_test(helperTests);
+	ut_run_test(integrationTests);
+	ut_run_test(inverseTests);
+	ut_run_test(matrixAssignmentTests);
+	ut_run_test(matrixMultTests);
+	ut_run_test(matrixScalarMultTests);
+	ut_run_test(setIdentityTests);
+	ut_run_test(sliceTests);
+	ut_run_test(squareMatrixTests);
+	ut_run_test(transposeTests);
+	ut_run_test(vectorTests);
+	ut_run_test(vector2Tests);
+	ut_run_test(vector3Tests);
+	ut_run_test(vectorAssignmentTests);
+
+	return (_tests_failed == 0);
+}
+
+
+ut_declare_test_c(test_matrix, MatrixTest)
+
+
+template class matrix::Quaternion<float>;
+template class matrix::Euler<float>;
+template class matrix::Dcm<float>;
+
+bool MatrixTest::attitudeTests(void)
+{
+	double eps = 1e-6;
+
+	// check data
+	Eulerf euler_check(0.1f, 0.2f, 0.3f);
+	Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
+	float dcm_data[] =  {
+		0.93629336f, -0.27509585f,  0.21835066f,
+		0.28962948f,  0.95642509f, -0.03695701f,
+		-0.19866933f,  0.0978434f,  0.97517033f
+	};
+	Dcmf dcm_check(dcm_data);
+
+	// euler ctor
+	ut_test(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f)));
+
+
+	// euler default ctor
+	Eulerf e;
+	Eulerf e_zero = zeros<float, 3, 1>();
+	ut_test(isEqual(e, e_zero));
+	ut_test(isEqual(e, e));
+
+	// euler vector ctor
+	Vector<float, 3> v;
+	v(0) = 0.1f;
+	v(1) = 0.2f;
+	v(2) = 0.3f;
+	Eulerf euler_copy(v);
+	ut_test(isEqual(euler_copy, euler_check));
+
+	// quaternion ctor
+	Quatf q0(1, 2, 3, 4);
+	Quatf q(q0);
+	ut_test(fabs(q(0) - 1) < eps);
+	ut_test(fabs(q(1) - 2) < eps);
+	ut_test(fabs(q(2) - 3) < eps);
+	ut_test(fabs(q(3) - 4) < eps);
+
+	// quat normalization
+	q.normalize();
+	ut_test(isEqual(q, Quatf(0.18257419f,  0.36514837f,
+				 0.54772256f,  0.73029674f)));
+	ut_test(isEqual(q0.unit(), q));
+
+	// quat default ctor
+	q = Quatf();
+	ut_test(isEqual(q, Quatf(1, 0, 0, 0)));
+
+	// euler to quaternion
+	q = Quatf(euler_check);
+	ut_test(isEqual(q, q_check));
+
+	// euler to dcm
+	Dcmf dcm(euler_check);
+	ut_test(isEqual(dcm, dcm_check));
+
+	// quaternion to euler
+	Eulerf e1(q_check);
+	ut_test(isEqual(e1, euler_check));
+
+	// quaternion to dcm
+	Dcmf dcm1(q_check);
+	ut_test(isEqual(dcm1, dcm_check));
+
+	// dcm default ctor
+	Dcmf dcm2;
+	SquareMatrix<float, 3> I = eye<float, 3>();
+	ut_test(isEqual(dcm2, I));
+
+	// dcm to euler
+	Eulerf e2(dcm_check);
+	ut_test(isEqual(e2, euler_check));
+
+	// dcm to quaterion
+	Quatf q2(dcm_check);
+	ut_test(isEqual(q2, q_check));
+
+	// constants
+	double deg2rad = M_PI / 180.0;
+	double rad2deg = 180.0 / M_PI;
+
+	// euler dcm round trip check
+	for (int roll = -90; roll <= 90; roll += 90) {
+		for (int pitch = -90; pitch <= 90; pitch += 90) {
+			for (int yaw = -179; yaw <= 180; yaw += 90) {
+				// note if theta = pi/2, then roll is set to zero
+				int roll_expected = roll;
+				int yaw_expected = yaw;
+
+				if (pitch == 90) {
+					roll_expected = 0;
+					yaw_expected = yaw - roll;
+
+				} else if (pitch == -90) {
+					roll_expected = 0;
+					yaw_expected = yaw + roll;
+				}
+
+				if (yaw_expected < -180) { yaw_expected += 360; }
+
+				if (yaw_expected > 180) { yaw_expected -= 360; }
+
+				//printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
+				Euler<double> euler_expected(
+					deg2rad * double(roll_expected),
+					deg2rad * double(pitch),
+					deg2rad * double(yaw_expected));
+				Euler<double> euler(
+					deg2rad * double(roll),
+					deg2rad * double(pitch),
+					deg2rad * double(yaw));
+				Dcm<double> dcm_from_euler(euler);
+				//dcm_from_euler.print();
+				Euler<double> euler_out(dcm_from_euler);
+				ut_test(isEqual(rad2deg * euler_expected, rad2deg * euler_out));
+
+				Eulerf eulerf_expected(
+					float(deg2rad)*float(roll_expected),
+					float(deg2rad)*float(pitch),
+					float(deg2rad)*float(yaw_expected));
+				Eulerf eulerf(float(deg2rad)*float(roll),
+					      float(deg2rad)*float(pitch),
+					      float(deg2rad)*float(yaw));
+				Dcm<float> dcm_from_eulerf(eulerf);
+				Euler<float> euler_outf(dcm_from_eulerf);
+				ut_test(isEqual(float(rad2deg)*eulerf_expected,
+						float(rad2deg)*euler_outf));
+			}
+		}
+	}
+
+	// quaterion copy ctors
+	float data_v4[] = {1, 2, 3, 4};
+	Vector<float, 4> v4(data_v4);
+	Quatf q_from_v(v4);
+	ut_test(isEqual(q_from_v, v4));
+
+	Matrix<float, 4, 1> m4(data_v4);
+	Quatf q_from_m(m4);
+	ut_test(isEqual(q_from_m, m4));
+
+	// quaternion derivate
+	Vector<float, 4> q_dot = q.derivative(Vector3f(1, 2, 3));
+
+	// quaternion product
+	Quatf q_prod_check(
+		0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
+	ut_test(isEqual(q_prod_check, q_check * q_check));
+	q_check *= q_check;
+	ut_test(isEqual(q_prod_check, q_check));
+
+	// Quaternion scalar multiplication
+	float scalar = 0.5;
+	Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);
+	Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar,
+				 3.0f * scalar,  4.0f * scalar);
+	Quatf q_scalar_mul_res = scalar * q_scalar_mul;
+	ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res));
+	Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
+	ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res2));
+	Quatf q_scalar_mul_res3(q_scalar_mul);
+	q_scalar_mul_res3 *= scalar;
+	ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res3));
+
+	// quaternion inverse
+	q = q_check.inversed();
+	ut_test(fabs(q_check(0) - q(0)) < eps);
+	ut_test(fabs(q_check(1) + q(1)) < eps);
+	ut_test(fabs(q_check(2) + q(2)) < eps);
+	ut_test(fabs(q_check(3) + q(3)) < eps);
+
+	q = q_check;
+	q.invert();
+	ut_test(fabs(q_check(0) - q(0)) < eps);
+	ut_test(fabs(q_check(1) + q(1)) < eps);
+	ut_test(fabs(q_check(2) + q(2)) < eps);
+	ut_test(fabs(q_check(3) + q(3)) < eps);
+
+	// rotate quaternion (nonzero rotation)
+	Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
+	Vector<float, 3> rot;
+	rot(0) = 1.0f;
+	rot(1) = rot(2) = 0.0f;
+	qI.rotate(rot);
+	Quatf q_true(cosf(1.0f / 2), sinf(1.0f / 2), 0.0f, 0.0f);
+	ut_test(fabs(qI(0) - q_true(0)) < eps);
+	ut_test(fabs(qI(1) - q_true(1)) < eps);
+	ut_test(fabs(qI(2) - q_true(2)) < eps);
+	ut_test(fabs(qI(3) - q_true(3)) < eps);
+
+	// rotate quaternion (zero rotation)
+	qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+	rot(0) = 0.0f;
+	rot(1) = rot(2) = 0.0f;
+	qI.rotate(rot);
+	q_true = Quatf(cosf(0.0f), sinf(0.0f), 0.0f, 0.0f);
+	ut_test(fabs(qI(0) - q_true(0)) < eps);
+	ut_test(fabs(qI(1) - q_true(1)) < eps);
+	ut_test(fabs(qI(2) - q_true(2)) < eps);
+	ut_test(fabs(qI(3) - q_true(3)) < eps);
+
+	// get rotation axis from quaternion (nonzero rotation)
+	q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f);
+	rot = q.to_axis_angle();
+	ut_test(fabs(rot(0)) < eps);
+	ut_test(fabs(rot(1) - 1.0f) < eps);
+	ut_test(fabs(rot(2)) < eps);
+
+	// get rotation axis from quaternion (zero rotation)
+	q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+	rot = q.to_axis_angle();
+	ut_test(fabs(rot(0)) < eps);
+	ut_test(fabs(rot(1)) < eps);
+	ut_test(fabs(rot(2)) < eps);
+
+	// from axis angle (zero rotation)
+	rot(0) = rot(1) = rot(2) = 0.0f;
+	q.from_axis_angle(rot, 0.0f);
+	q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+	ut_test(fabs(q(0) - q_true(0)) < eps);
+	ut_test(fabs(q(1) - q_true(1)) < eps);
+	ut_test(fabs(q(2) - q_true(2)) < eps);
+	ut_test(fabs(q(3) - q_true(3)) < eps);
+
+	return true;
+}
+
+bool MatrixTest::filterTests(void)
+{
+	const size_t n_x = 6;
+	const size_t n_y = 5;
+	SquareMatrix<float, n_x> P = eye<float, n_x>();
+	SquareMatrix<float, n_y> R = eye<float, n_y>();
+	Matrix<float, n_y, n_x> C;
+	C.setIdentity();
+	float data[] = {1, 2, 3, 4, 5};
+	Vector<float, n_y> r(data);
+
+	Vector<float, n_x> dx;
+	SquareMatrix<float, n_x> dP;
+	float beta = 0;
+	kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
+
+	float data_check[] = {0.5, 1, 1.5, 2, 2.5, 0};
+	Vector<float, n_x> dx_check(data_check);
+	ut_test(isEqual(dx, dx_check));
+
+	return true;
+}
+
+bool MatrixTest::helperTests(void)
+{
+	ut_test(fabs(wrap_pi(4.0) - (4.0 - 2 * M_PI)) < 1e-5);
+	ut_test(fabs(wrap_pi(-4.0) - (-4.0 + 2 * M_PI)) < 1e-5);
+	ut_test(fabs(wrap_pi(3.0) - (3.0)) < 1e-3);
+	wrap_pi(NAN);
+
+	Vector3f a(1, 2, 3);
+	Vector3f b(4, 5, 6);
+	ut_test(isEqual(a, a));
+
+	return true;
+}
+
+
+Vector<float, 6> f(float t, const Matrix<float, 6, 1> &y, const Matrix<float, 3, 1> &u);
+
+Vector<float, 6> f(float t, const Matrix<float, 6, 1> &y, const Matrix<float, 3, 1> &u)
+{
+	float v = -sinf(t);
+	return v * ones<float, 6, 1>();
+}
+
+bool MatrixTest::integrationTests(void)
+{
+	Vector<float, 6> y = ones<float, 6, 1>();
+	Vector<float, 3> u = ones<float, 3, 1>();
+	float t0 = 0;
+	float tf = 2;
+	float h = 0.001f;
+	integrate_rk4(f, y, u, t0, tf, h, y);
+	float v = 1 + cosf(tf) - cosf(t0);
+	ut_test(isEqual(y, (ones<float, 6, 1>()*v)));
+
+	return true;
+}
+
+template class matrix::SquareMatrix<float, 3>;
+
+bool MatrixTest::inverseTests(void)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	float data_check[9] = {
+		-0.4f, -0.8f,  0.6f,
+		-0.4f,  4.2f, -2.4f,
+		0.6f, -2.8f,  1.6f
+	};
+
+	SquareMatrix<float, 3> A(data);
+	SquareMatrix<float, 3> A_I = inv(A);
+	SquareMatrix<float, 3> A_I_check(data_check);
+
+	float eps = 1e-5;
+
+	ut_test((A_I - A_I_check).abs().max() < eps);
+
+	SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
+	inv(zero_test);
+
+	return true;
+}
+
+bool MatrixTest::matrixAssignmentTests(void)
+{
+	Matrix3f m;
+	m.setZero();
+	m(0, 0) = 1;
+	m(0, 1) = 2;
+	m(0, 2) = 3;
+	m(1, 0) = 4;
+	m(1, 1) = 5;
+	m(1, 2) = 6;
+	m(2, 0) = 7;
+	m(2, 1) = 8;
+	m(2, 2) = 9;
+
+	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+	Matrix3f m2(data);
+
+	double eps = 1e-6f;
+
+	for (int i = 0; i < 9; i++) {
+		ut_test(fabs(data[i] - m2.data()[i]) < eps);
+	}
+
+	float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	Matrix3f m3(data_times_2);
+
+	ut_test(isEqual(m, m2));
+	ut_test(!(m == m3));
+
+	m2 *= 2;
+	ut_test(m2 == m3);
+
+	m2 /= 2;
+	m2 -= 1;
+	float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+	ut_test(Matrix3f(data_minus_1) == m2);
+
+	m2 += 1;
+	ut_test(Matrix3f(data) == m2);
+
+	m3 -= m2;
+
+	ut_test(m3 == m2);
+
+	float data_row_02_swap[9] = {
+		7, 8, 9,
+		4, 5, 6,
+		1, 2, 3,
+	};
+
+	float data_col_02_swap[9] = {
+		3, 2, 1,
+		6, 5, 4,
+		9, 8, 7
+	};
+
+	Matrix3f m4(data);
+
+	ut_test(-m4 == m4 * (-1));
+
+	m4.swapCols(0, 2);
+	ut_test(m4 == Matrix3f(data_col_02_swap));
+	m4.swapCols(0, 2);
+	m4.swapRows(0, 2);
+	ut_test(m4 == Matrix3f(data_row_02_swap));
+	ut_test(fabs(m4.min() - 1) < 1e-5);
+
+	Scalar<float> s;
+	s = 1;
+	ut_test(fabs(s - 1) < 1e-5);
+
+	Matrix<float, 1, 1> m5 = s;
+	ut_test(fabs(m5(0, 0) - s) < 1e-5);
+
+	Matrix<float, 2, 2> m6;
+	m6.setRow(0, Vector2f(1, 1));
+	m6.setCol(0, Vector2f(1, 1));
+
+	return true;
+}
+
+bool MatrixTest::matrixMultTests(void)
+{
+	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
+	Matrix3f A(data);
+	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
+	Matrix3f A_I(data_check);
+	Matrix3f I;
+	I.setIdentity();
+	Matrix3f R = A * A_I;
+	ut_test(isEqual(R, I));
+
+	Matrix3f R2 = A;
+	R2 *= A_I;
+	ut_test(isEqual(R2, I));
+
+
+	Matrix3f A2 = eye<float, 3>() * 2;
+	Matrix3f B = A2.emult(A2);
+	Matrix3f B_check = eye<float, 3>() * 4;
+	ut_test(isEqual(B, B_check));
+
+	return true;
+}
+
+bool MatrixTest::matrixScalarMultTests(void)
+{
+	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+	Matrix3f A(data);
+	A = A * 2;
+	float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	Matrix3f A_check(data_check);
+	ut_test(isEqual(A, A_check));
+
+	return true;
+}
+
+
+template class matrix::Matrix<float, 3, 3>;
+
+bool MatrixTest::setIdentityTests(void)
+{
+	Matrix3f A;
+	A.setIdentity();
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			if (i == j) {
+				ut_test(fabs(A(i, j) -  1) < 1e-7);
+
+			} else {
+				ut_test(fabs(A(i, j) -  0) < 1e-7);
+			}
+		}
+	}
+
+	return true;
+}
+
+bool MatrixTest::sliceTests(void)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	float data_check[6] = {
+		4, 5, 6,
+		7, 8, 10
+	};
+	SquareMatrix<float, 3> A(data);
+	Matrix<float, 2, 3> B_check(data_check);
+	Matrix<float, 2, 3> B(A.slice<2, 3>(1, 0));
+	ut_test(isEqual(B, B_check));
+
+	float data_2[4] = {
+		11, 12,
+		13, 14
+	};
+
+	Matrix<float, 2, 2> C(data_2);
+	A.set(C, 1, 1);
+
+	float data_2_check[9] = {
+		0, 2, 3,
+		4, 11, 12,
+		7, 13, 14
+	};
+	Matrix<float, 3, 3> D(data_2_check);
+	ut_test(isEqual(A, D));
+
+	return true;
+}
+
+
+bool MatrixTest::squareMatrixTests(void)
+{
+	float data[9] = {1, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	SquareMatrix<float, 3> A(data);
+	Vector3<float> diag_check(1, 5, 10);
+
+	ut_test(isEqual(A.diag(), diag_check));
+
+	float data_check[9] = {
+		1.01158503f,  0.02190432f,  0.03238144f,
+		0.04349195f,  1.05428524f,  0.06539627f,
+		0.07576783f,  0.08708946f,  1.10894048f
+	};
+
+	float dt = 0.01f;
+	SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A * dt), 5);
+	SquareMatrix<float, 3> eA_check(data_check);
+
+	float eps = 1e-3;
+	ut_test((eA - eA_check).abs().max() < eps);
+
+	return true;
+}
+
+bool MatrixTest::transposeTests(void)
+{
+	float data[6] = {1, 2, 3, 4, 5, 6};
+	Matrix<float, 2, 3> A(data);
+	Matrix<float, 3, 2> A_T = A.transpose();
+	float data_check[6] = {1, 4, 2, 5, 3, 6};
+	Matrix<float, 3, 2> A_T_check(data_check);
+	ut_test(isEqual(A_T, A_T_check));
+
+	return true;
+}
+
+bool MatrixTest::vectorTests(void)
+{
+	float data1[] = {1, 2, 3, 4, 5};
+	float data2[] = {6, 7, 8, 9, 10};
+	Vector<float, 5> v1(data1);
+	ut_test(fabs(v1.norm() - 7.416198487095663f) < 1e-5);
+	Vector<float, 5> v2(data2);
+	ut_test(fabs(v1.dot(v2) - 130.0f) < 1e-5);
+	v2.normalize();
+	Vector<float, 5> v3(v2);
+	ut_test(v2 == v3);
+	float data1_sq[] = {1, 4, 9, 16, 25};
+	Vector<float, 5> v4(data1_sq);
+	ut_test(v1 == v4.pow(0.5));
+
+	return true;
+}
+
+bool MatrixTest::vector2Tests(void)
+{
+	Vector2f a(1, 0);
+	Vector2f b(0, 1);
+	ut_test(fabs(a % b - 1.0f) < 1e-5);
+
+	Vector2f c;
+	ut_test(fabs(c(0) - 0) < 1e-5);
+	ut_test(fabs(c(1) - 0) < 1e-5);
+
+	Matrix<float, 2, 1> d(a);
+	ut_test(fabs(d(0, 0) - 1) < 1e-5);
+	ut_test(fabs(d(1, 0) - 0) < 1e-5);
+
+	Vector2f e(d);
+	ut_test(fabs(e(0) - 1) < 1e-5);
+	ut_test(fabs(e(1) - 0) < 1e-5);
+
+	float data[] = {4, 5};
+	Vector2f f(data);
+	ut_test(fabs(f(0) - 4) < 1e-5);
+	ut_test(fabs(f(1) - 5) < 1e-5);
+	return true;
+}
+
+bool MatrixTest::vector3Tests(void)
+{
+	Vector3f a(1, 0, 0);
+	Vector3f b(0, 1, 0);
+	Vector3f c = a.cross(b);
+	ut_test(c == Vector3f(0, 0, 1));
+	c = a % b;
+	ut_test(c == Vector3f(0, 0, 1));
+	Matrix<float, 3, 1> d(c);
+	Vector3f e(d);
+	ut_test(e == d);
+	float data[] = {4, 5, 6};
+	Vector3f f(data);
+	ut_test(f == Vector3f(4, 5, 6));
+	return true;
+}
+
+bool MatrixTest::vectorAssignmentTests(void)
+{
+	Vector3f v;
+	v(0) = 1;
+	v(1) = 2;
+	v(2) = 3;
+
+	static const float eps = 1e-7f;
+
+	ut_test(fabsf(v(0) - 1) < eps);
+	ut_test(fabsf(v(1) - 2) < eps);
+	ut_test(fabsf(v(2) - 3) < eps);
+
+	Vector3f v2(4, 5, 6);
+
+	ut_test(fabsf(v2(0) - 4) < eps);
+	ut_test(fabsf(v2(1) - 5) < eps);
+	ut_test(fabsf(v2(2) - 6) < eps);
+
+	SquareMatrix<float, 3> m = diag(Vector3f(1, 2, 3));
+	ut_test(fabsf(m(0, 0) - 1) < eps);
+	ut_test(fabsf(m(1, 1) - 2) < eps);
+	ut_test(fabsf(m(2, 2) - 3) < eps);
+
+	return true;
+}
@@ -85,22 +85,22 @@ int test_mixer(int argc, char *argv[])
 	uint16_t servo_predicted[output_max];
 	int16_t reverse_pwm_mask = 0;

-	PX4_INFO("testing mixer");
+	//PX4_INFO("testing mixer");

+#if !defined(CONFIG_ARCH_BOARD_SITL)
 	const char *filename = "/etc/mixers/IO_pass.mix";
+#else
+	const char *filename = "../../../../ROMFS/px4fmu_test/mixers/IO_pass.mix";
+#endif

-	if (argc > 1) {
-		filename = argv[1];
-	}
-
-	PX4_INFO("loading: %s", filename);
+	//PX4_INFO("loading: %s", filename);

 	char		buf[2048];

 	load_mixer_file(filename, &buf[0], sizeof(buf));
 	unsigned loaded = strlen(buf);

-	fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+	//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);

 	/* load the mixer in chunks, like
 	 * in the case of a remote load,
@@ -114,11 +114,7 @@ int test_mixer(int argc, char *argv[])
 	/* load at once test */
 	unsigned xx = loaded;
 	mixer_group.load_from_buf(&buf[0], xx);
-	PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
-
-	if (mixer_group.count() != 8) {
-		return 1;
-	}
+	//ASSERT_EQ(mixer_group.count(), 8);

 	unsigned empty_load = 2;
 	char empty_buf[2];
@@ -126,7 +122,9 @@ int test_mixer(int argc, char *argv[])
 	empty_buf[1] = '\0';
 	mixer_group.reset();
 	mixer_group.load_from_buf(&empty_buf[0], empty_load);
-	PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+	//PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+
+	//ASSERT_NE(empty_load, 0);

 	if (empty_load != 0) {
 		return 1;
@@ -140,7 +138,7 @@ int test_mixer(int argc, char *argv[])

 	unsigned transmitted = 0;

-	PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
+	//PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);

 	while (transmitted < loaded) {

@@ -155,7 +153,7 @@ int test_mixer(int argc, char *argv[])
 		memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
 		mixer_text_length += text_length;
 		mixer_text[mixer_text_length] = '\0';
-		fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
+		//fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);

 		/* process the text buffer, adding new mixers as their descriptions can be parsed */
 		unsigned resid = mixer_text_length;
@@ -163,7 +161,7 @@ int test_mixer(int argc, char *argv[])

 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
-			fprintf(stderr, "used %u", mixer_text_length - resid);
+			//fprintf(stderr, "used %u", mixer_text_length - resid);

 			/* copy any leftover text to the base of the buffer for re-use */
 			if (resid > 0) {
@@ -176,7 +174,7 @@ int test_mixer(int argc, char *argv[])
 		transmitted += text_length;
 	}

-	PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
+	//PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());

 	if (mixer_group.count() != 8) {
 		return 1;
@@ -198,7 +196,8 @@ int test_mixer(int argc, char *argv[])
 		r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
 	}

-	PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
+	//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
+
 	/* mix */
 	should_prearm = true;
 	mixed = mixer_group.mix(&outputs[0], output_max, NULL);
@@ -209,7 +208,7 @@ int test_mixer(int argc, char *argv[])
 	//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
 	for (unsigned i = 0; i < mixed; i++) {

-		fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
+		//fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);

 		if (i != actuator_controls_s::INDEX_THROTTLE) {
 			if (r_page_servos[i] < r_page_servo_control_min[i]) {
@@ -233,7 +232,7 @@ int test_mixer(int argc, char *argv[])
 		actuator_controls[i] = 0.1f;
 	}

-	PX4_INFO("ARMING TEST: STARTING RAMP");
+	//PX4_INFO("ARMING TEST: STARTING RAMP");
 	unsigned sleep_quantum_us = 10000;

 	hrt_abstime starttime = hrt_absolute_time();
@@ -250,7 +249,7 @@ int test_mixer(int argc, char *argv[])
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
 		for (unsigned i = 0; i < mixed; i++) {

-			fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
+			//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);

 			/* check mixed outputs to be zero during init phase */
 			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
@@ -274,7 +273,7 @@ int test_mixer(int argc, char *argv[])
 		}
 	}

-	PX4_INFO("ARMING TEST: NORMAL OPERATION");
+	//PX4_INFO("ARMING TEST: NORMAL OPERATION");

 	for (int j = -jmax; j <= jmax; j++) {

@@ -292,7 +291,7 @@ int test_mixer(int argc, char *argv[])
 			       r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);

-		fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
+		//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);

 		for (unsigned i = 0; i < mixed; i++) {
 			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
@@ -306,7 +305,7 @@ int test_mixer(int argc, char *argv[])
 		}
 	}

-	PX4_INFO("ARMING TEST: DISARMING");
+	//PX4_INFO("ARMING TEST: DISARMING");

 	starttime = hrt_absolute_time();
 	sleepcount = 0;
@@ -324,7 +323,7 @@ int test_mixer(int argc, char *argv[])
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
 		for (unsigned i = 0; i < mixed; i++) {

-			fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
+			//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);

 			/* check mixed outputs to be zero during init phase */
 			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@@ -337,14 +336,14 @@ int test_mixer(int argc, char *argv[])
 		sleepcount++;

 		if (sleepcount % 10 == 0) {
-			printf(".");
-			fflush(stdout);
+			//printf(".");
+			//fflush(stdout);
 		}
 	}

-	printf("\n");
+	//printf("\n");

-	PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
+	//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");

 	starttime = hrt_absolute_time();
 	sleepcount = 0;
@@ -366,7 +365,7 @@ int test_mixer(int argc, char *argv[])

 			/* check ramp */

-			fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
+			//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);

 			if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
 			    (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
@@ -388,46 +387,42 @@ int test_mixer(int argc, char *argv[])
 		sleepcount++;

 		if (sleepcount % 10 == 0) {
-			printf(".");
-			fflush(stdout);
+			//	printf(".");
+			//	fflush(stdout);
 		}
 	}

-	printf("\n");
+	//printf("\n");

 	/* load multirotor at once test */
 	mixer_group.reset();

-	if (argc > 2) {
-		filename = argv[2];
-
-	} else {
-		filename = "/etc/mixers/quad_test.mix";
-	}
+#if !defined(CONFIG_ARCH_BOARD_SITL)
+	filename = "/etc/mixers/quad_test.mix";
+#else
+	filename = "../../../../ROMFS/px4fmu_test/mixers/quad_test.mix";
+#endif

 	load_mixer_file(filename, &buf[0], sizeof(buf));
 	loaded = strlen(buf);

-	fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+	//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);

 	unsigned mc_loaded = loaded;
 	mixer_group.load_from_buf(&buf[0], mc_loaded);
-	PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
+	//PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());

 	if (mixer_group.count() != 5) {
 		PX4_ERR("FAIL: Quad test mixer load failed");
 		return 1;
 	}

-	PX4_INFO("SUCCESS: No errors in mixer test");
+	//PX4_INFO("SUCCESS: No errors in mixer test");
 	return 0;
 }

 static int
-mixer_callback(uintptr_t handle,
-	       uint8_t control_group,
-	       uint8_t control_index,
-	       float &control)
+mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
 {
 	if (control_group != 0) {
 		return -1;
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 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_config.h>
+#include <px4_posix.h>
+
+#include <systemlib/perf_counter.h>
+
+#include "tests.h"
+
+int
+test_perf(int argc, char *argv[])
+{
+	perf_counter_t cc = perf_alloc(PC_COUNT, "test_count");
+	perf_counter_t ec = perf_alloc(PC_ELAPSED, "test_elapsed");
+
+	if ((cc == NULL) || (ec == NULL)) {
+		printf("perf: counter alloc failed\n");
+		return 1;
+	}
+
+	perf_begin(ec);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	printf("perf: expect count of 4\n");
+	perf_print_counter(cc);
+	perf_end(ec);
+	printf("perf: expect count of 1\n");
+	perf_print_counter(ec);
+	printf("perf: expect at least two counters\n");
+	perf_print_all(0);
+
+	perf_free(cc);
+	perf_free(ec);
+
+	return OK;
+}
@@ -57,40 +57,6 @@
 #include <math.h>
 #include <float.h>

-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
 int test_uart_baudchange(int argc, char *argv[])
 {
 	int uart2_nwrite = 0;
@@ -56,40 +56,6 @@
 #include <float.h>
 #include <drivers/drv_hrt.h>

-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
 static void *receive_loop(void *arg)
 {
 	int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY);
@@ -70,50 +70,48 @@
 #  endif
 #endif

-/****************************************************************************
- * Public Types
- ****************************************************************************/
-
-/****************************************************************************
- * Public Variables
- ****************************************************************************/
-
-/****************************************************************************
- * Public Function Prototypes
- ****************************************************************************/
-
 __BEGIN_DECLS

-extern int	test_sensors(int argc, char *argv[]);
-extern int	test_gpio(int argc, char *argv[]);
-extern int	test_hrt(int argc, char *argv[]);
-extern int	test_tone(int argc, char *argv[]);
-extern int	test_led(int argc, char *argv[]);
 extern int	test_adc(int argc, char *argv[]);
-extern int	test_int(int argc, char *argv[]);
-extern int	test_float(int argc, char *argv[]);
-extern int	test_ppm(int argc, char *argv[]);
-extern int	test_servo(int argc, char *argv[]);
-extern int	test_ppm_loopback(int argc, char *argv[]);
-extern int	test_uart_loopback(int argc, char *argv[]);
-extern int	test_uart_baudchange(int argc, char *argv[]);
-extern int	test_cpuload(int argc, char *argv[]);
-extern int	test_uart_send(int argc, char *argv[]);
-extern int	test_sleep(int argc, char *argv[]);
-extern int	test_time(int argc, char *argv[]);
-extern int	test_uart_console(int argc, char *argv[]);
-extern int	test_hott_telemetry(int argc, char *argv[]);
-extern int	test_jig_voltages(int argc, char *argv[]);
-extern int	test_param(int argc, char *argv[]);
+extern int	test_autodeclination(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[]);
 extern int	test_file2(int argc, char *argv[]);
-extern int	test_mixer(int argc, char *argv[]);
-extern int	test_rc(int argc, char *argv[]);
-extern int	test_conv(int argc, char *argv[]);
-extern int	test_mount(int argc, char *argv[]);
+extern int	test_float(int argc, char *argv[]);
+extern int	test_gpio(int argc, char *argv[]);
+extern int	test_hott_telemetry(int argc, char *argv[]);
+extern int	test_hrt(int argc, char *argv[]);
+extern int	test_int(int argc, char *argv[]);
+extern int	test_jig_voltages(int argc, char *argv[]);
+extern int	test_led(int argc, char *argv[]);
 extern int	test_mathlib(int argc, char *argv[]);
-extern int	test_eigen(int argc, char *argv[]);
+extern int	test_matrix(int argc, char *argv[]);
+extern int	test_mixer(int argc, char *argv[]);
+extern int	test_mount(int argc, char *argv[]);
+extern int	test_param(int argc, char *argv[]);
+extern int	test_perf(int argc, char *argv[]);
+extern int	test_ppm(int argc, char *argv[]);
+extern int	test_ppm_loopback(int argc, char *argv[]);
+extern int	test_rc(int argc, char *argv[]);
+extern int	test_sensors(int argc, char *argv[]);
+extern int	test_servo(int argc, char *argv[]);
+extern int	test_sleep(int argc, char *argv[]);
+extern int	test_time(int argc, char *argv[]);
+extern int	test_tone(int argc, char *argv[]);
+extern int	test_uart_baudchange(int argc, char *argv[]);
+extern int	test_uart_console(int argc, char *argv[]);
+extern int	test_uart_loopback(int argc, char *argv[]);
+extern int	test_uart_send(int argc, char *argv[]);
+
+/* external */
+extern int commander_tests_main(int argc, char *argv[]);
+extern int mavlink_tests_main(int argc, char *argv[]);
+extern int controllib_test_main(int argc, char *argv[]);
+extern int uorb_tests_main(int argc, char *argv[]);
+extern int rc_tests_main(int argc, char *argv[]);
+extern int sf0x_tests_main(int argc, char *argv[]);
+

 __END_DECLS

@@ -38,20 +38,17 @@
 * @author Lorenz Meier <lm@inf.ethz.ch>
 */

+#include "tests.h"
+
 #include <px4_config.h>

 #include <sys/types.h>

 #include <stdio.h>
-#include <stdlib.h>
 #include <string.h>
-#include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>

-#include <arch/board/board.h>
-#include <systemlib/perf_counter.h>
-
 // Not using Eigen at the moment
 #define TESTS_EIGEN_DISABLE

@@ -62,8 +59,9 @@
 ****************************************************************************/

 static int test_help(int argc, char *argv[]);
+static int test_runner(unsigned option);
+
 static int test_all(int argc, char *argv[]);
-static int test_perf(int argc, char *argv[]);
 static int test_jig(int argc, char *argv[]);

 /****************************************************************************
@@ -78,45 +76,52 @@ const struct {
 #define OPT_NOALLTEST	(1<<1)
 #define OPT_NOJIGTEST	(1<<2)
 } tests[] = {
-#ifdef __PX4_NUTTX
-	{"led",			test_led,	0},
-	{"time",		test_time,	OPT_NOJIGTEST},
-	{"sensors",		test_sensors,	0},
-	{"adc",			test_adc,	OPT_NOJIGTEST},
-#endif /* __PX4_NUTTX */
-	{"int",			test_int,	0},
-	{"float",		test_float,	0},
-	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
-	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
-	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"hott_telemetry",	test_hott_telemetry,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"tone",		test_tone,	0},
-	{"sleep",		test_sleep,	OPT_NOJIGTEST},
-	{"perf",		test_perf,	OPT_NOJIGTEST},
+	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{"all",			test_all,	OPT_NOALLTEST | OPT_NOJIGTEST},
 	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"param",		test_param,	0},
+#ifdef __PX4_NUTTX
+	{"adc",			test_adc,	OPT_NOJIGTEST},
+	{"led",			test_led,	0},
+	{"sensors",		test_sensors,	0},
+	{"time",		test_time,	OPT_NOJIGTEST},
+	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST},
+#else
+	{"rc",			rc_tests_main,	0},
+#endif /* __PX4_NUTTX */
+
+	/* external tests */
+	{"commander",		commander_tests_main,	0},
+	{"controllib",		controllib_test_main,	0},
+	//{"mavlink",		mavlink_tests_main,	0}, // TODO: fix mavlink_tests
+	{"sf0x",			sf0x_tests_main,	0},
+	{"uorb",		uorb_tests_main,	0},
+
+	{"autodeclination",		test_autodeclination,	0},
 	{"bson",		test_bson,	0},
+	{"conv",		test_conv, 0},
 	{"file",		test_file,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"file2",		test_file2,	OPT_NOJIGTEST},
-	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
-#ifndef TESTS_MATHLIB_DISABLE
+	{"float",		test_float,	0},
+	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"hott_telemetry",	test_hott_telemetry,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"int",			test_int,	0},
+	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
 	{"mathlib",		test_mathlib,	0},
-#endif
-#ifndef TESTS_EIGEN_DISABLE
-	{"eigen",		test_eigen,	OPT_NOJIGTEST},
-#endif
-	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
+	{"matrix",		test_matrix,	0},
+	{"mixer",		test_mixer,	OPT_NOJIGTEST},
+	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"param",		test_param,	0},
+	{"perf",		test_perf,	OPT_NOJIGTEST},
+	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
+	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"sleep",		test_sleep,	OPT_NOJIGTEST},
+	{"tone",		test_tone,	0},
+	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{NULL,			NULL, 		0}
 };

@@ -138,18 +143,30 @@ test_help(int argc, char *argv[])

 static int
 test_all(int argc, char *argv[])
+{
+	return test_runner(OPT_NOALLTEST);
+}
+
+static int
+test_jig(int argc, char *argv[])
+{
+	return test_runner(OPT_NOJIGTEST);
+}
+
+static int
+test_runner(unsigned option)
 {
 	unsigned	i;
 	char		*args[2] = {"all", NULL};
 	unsigned int failcount = 0;
 	unsigned int testcount = 0;
-	bool		passed[NTESTS];
+	unsigned		passed[NTESTS];

 	printf("\nRunning all tests...\n\n");

 	for (i = 0; tests[i].name; i++) {
 		/* Only run tests that are not excluded */
-		if (!(tests[i].options & OPT_NOALLTEST)) {
+		if (!(tests[i].options & option)) {
 			for (int j = 0; j < 80; j++) {
 				printf("-");
 			}
@@ -164,12 +181,12 @@ test_all(int argc, char *argv[])
 				fprintf(stderr, "  [%s] \t\tFAIL\n", tests[i].name);
 				fflush(stderr);
 				failcount++;
-				passed[i] = false;
+				passed[i] = 0;

 			} else {
 				printf("  [%s] \t\tPASS\n", tests[i].name);
 				fflush(stdout);
-				passed[i] = true;
+				passed[i] = 1;
 			}

 			for (int j = 0; j < 80; j++) {
@@ -185,7 +202,7 @@ test_all(int argc, char *argv[])
 	/* Print summary */
 	printf("\n");

-	for (int j = 0; j < 80; j++) {
+	for (unsigned j = 0; j < 80; j++) {
 		printf("#");
 	}

@@ -215,12 +232,12 @@ test_all(int argc, char *argv[])
 	printf("\n");

 	/* Print failed tests */
-	if (failcount > 0) { printf(" Failed tests:\n\n"); }
+	if (failcount > 0) {
+		printf(" Failed tests:\n\n");
+	}

-	unsigned int k;
-
-	for (k = 0; k < i; k++) {
-		if (!passed[k] && !(tests[k].options & OPT_NOALLTEST)) {
+	for (unsigned k = 0; k < i; k++) {
+		if (!passed[k] && !(tests[k].options & option)) {
 			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
 		}
 	}
@@ -230,129 +247,6 @@ test_all(int argc, char *argv[])
 	return (failcount > 0);
 }

-static int
-test_perf(int argc, char *argv[])
-{
-	perf_counter_t	cc, ec;
-
-	cc = perf_alloc(PC_COUNT, "test_count");
-	ec = perf_alloc(PC_ELAPSED, "test_elapsed");
-
-	if ((cc == NULL) || (ec == NULL)) {
-		printf("perf: counter alloc failed\n");
-		return 1;
-	}
-
-	perf_begin(ec);
-	perf_count(cc);
-	perf_count(cc);
-	perf_count(cc);
-	perf_count(cc);
-	printf("perf: expect count of 4\n");
-	perf_print_counter(cc);
-	perf_end(ec);
-	printf("perf: expect count of 1\n");
-	perf_print_counter(ec);
-	printf("perf: expect at least two counters\n");
-	perf_print_all(0);
-
-	perf_free(cc);
-	perf_free(ec);
-
-	return OK;
-}
-
-int test_jig(int argc, char *argv[])
-{
-	unsigned	i;
-	char		*args[2] = {"jig", NULL};
-	unsigned int failcount = 0;
-	unsigned int testcount = 0;
-	bool		passed[NTESTS];
-
-	printf("\nRunning all tests...\n\n");
-
-	for (i = 0; tests[i].name; i++) {
-		/* Only run tests that are not excluded */
-		if (!(tests[i].options & OPT_NOJIGTEST)) {
-			for (int j = 0; j < 80; j++) {
-				printf("-");
-			}
-
-			printf("\n");
-
-			printf("  [%s] \t\tSTARTING TEST\n", tests[i].name);
-			fflush(stdout);
-
-			/* Execute test */
-			if (tests[i].fn(1, args) != 0) {
-				fprintf(stderr, "  [%s] \t\tFAIL\n", tests[i].name);
-				fflush(stderr);
-				failcount++;
-				passed[i] = false;
-
-			} else {
-				printf("  [%s] \t\tPASS\n", tests[i].name);
-				fflush(stdout);
-				passed[i] = true;
-			}
-
-			for (int j = 0; j < 80; j++) {
-				printf("-");
-			}
-
-			printf("\n");
-
-			testcount++;
-		}
-	}
-
-	/* Print summary */
-	printf("\n");
-
-	for (int j = 0; j < 80; j++) {
-		printf("-");
-	}
-
-	printf("\n\n");
-
-	printf("     T E S T    S U M M A R Y\n\n");
-
-	if (failcount == 0) {
-		printf("  ______     __         __            ______     __  __    \n");
-		printf(" /\\  __ \\   /\\ \\       /\\ \\          /\\  __ \\   /\\ \\/ /    \n");
-		printf(" \\ \\  __ \\  \\ \\ \\____  \\ \\ \\____     \\ \\ \\/\\ \\  \\ \\  _\"-.  \n");
-		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
-		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
-		printf("\n");
-		printf(" All tests passed (%d of %d)\n", testcount, testcount);
-
-	} else {
-		printf("  ______   ______     __     __ \n");
-		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
-		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
-		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
-		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
-		printf("\n");
-		printf(" Some tests failed (%d of %d)\n", failcount, testcount);
-	}
-
-	printf("\n");
-
-	/* Print failed tests */
-	if (failcount > 0) { printf(" Failed tests:\n\n"); }
-
-	for (int k = 0; k < i; k++) {
-		if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) {
-			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
-		}
-	}
-
-	fflush(stdout);
-
-	return 0;
-}
-
 __EXPORT int tests_main(int argc, char *argv[]);

 /**
@@ -360,19 +254,17 @@ __EXPORT int tests_main(int argc, char *argv[]);
 */
 int tests_main(int argc, char *argv[])
 {
-	unsigned	i;
-
 	if (argc < 2) {
 		printf("tests: missing test name - 'tests help' for a list of tests\n");
 		return 1;
 	}

-	for (i = 0; tests[i].name; i++) {
+	for (unsigned i = 0; tests[i].name; i++) {
 		if (!strcmp(tests[i].name, argv[1])) {
 			return tests[i].fn(argc - 1, argv + 1);
 		}
 	}

 	printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]);
-	return ERROR;
+	return 1;
 }