systemcmds/tests: split out microbenchmarks and remove obsolete tests

- reorganize Jenkins HIL tests

src/systemcmds/microbench/CMakeLists.txt

@@ -0,0 +1,53 @@
+############################################################################
+#
+#   Copyright (c) 2015-2021 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 systemcmds__microbench
+	MAIN microbench
+	STACK_MAIN 4096
+	COMPILE_FLAGS
+		-Wno-double-promotion
+		-Wno-unused-but-set-variable
+		-Wno-unused-variable
+		-Wno-write-strings
+	SRCS
+		microbench_main.cpp
+
+		test_microbench_atomic.cpp
+		test_microbench_hrt.cpp
+		test_microbench_math.cpp
+		test_microbench_matrix.cpp
+		test_microbench_uorb.cpp
+
+	DEPENDS
+)

src/systemcmds/microbench/microbench_main.cpp

@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2015-2021 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_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+
+__BEGIN_DECLS
+
+extern int test_microbench_atomic(int argc, char *argv[]);
+extern int test_microbench_hrt(int argc, char *argv[]);
+extern int test_microbench_math(int argc, char *argv[]);
+extern int test_microbench_matrix(int argc, char *argv[]);
+extern int test_microbench_uorb(int argc, char *argv[]);
+
+__END_DECLS
+
+static int microbench_help(int argc, char *argv[]);
+static int microbench_all(int argc, char *argv[]);
+static int microbench_runner(unsigned option);
+
+const struct {
+	const char 	*name;
+	int	(* fn)(int argc, char *argv[]);
+	unsigned	options;
+#define OPT_NOHELP	(1<<0)
+#define OPT_NOALLTEST	(1<<1)
+} microbenchmarks[] = {
+	{"help",		microbench_help,		OPT_NOALLTEST | OPT_NOHELP},
+	{"all",		microbench_all,		OPT_NOALLTEST},
+
+	{"microbench_atomic",	test_microbench_atomic,	0},
+	{"microbench_hrt",	test_microbench_hrt,	0},
+	{"microbench_math",	test_microbench_math,	0},
+	{"microbench_matrix",	test_microbench_matrix,	0},
+	{"microbench_uorb",	test_microbench_uorb,	0},
+
+	{NULL,			NULL, 		0}
+};
+
+#define NMICROBENCHMARKS (sizeof(microbenchmarks) / sizeof(microbenchmarks[0]))
+
+static int microbench_help(int argc, char *argv[])
+{
+	printf("Available tests:\n");
+
+	for (int i = 0; microbenchmarks[i].name; i++) {
+		printf("  %s\n", microbenchmarks[i].name);
+	}
+
+	return 0;
+}
+
+static int microbench_all(int argc, char *argv[])
+{
+	return microbench_runner(OPT_NOALLTEST);
+}
+
+static int microbench_runner(unsigned option)
+{
+	size_t i;
+	char *args[2] = {"all", NULL};
+	unsigned int failcount = 0;
+	unsigned int testcount = 0;
+	unsigned int passed[NMICROBENCHMARKS];
+
+	printf("\nRunning all microbenchmarks...\n\n");
+
+	for (i = 0; microbenchmarks[i].name; i++) {
+		// Only run tests that are not excluded.
+		if (!(microbenchmarks[i].options & option)) {
+			for (int j = 0; j < 80; j++) {
+				printf("-");
+			}
+
+			printf("\n  [%s] \t\tSTARTING TEST\n", microbenchmarks[i].name);
+			fflush(stdout);
+
+			/* Execute test */
+			if (microbenchmarks[i].fn(1, args) != 0) {
+				fprintf(stderr, "  [%s] \t\tFAIL\n", microbenchmarks[i].name);
+				fflush(stderr);
+				failcount++;
+				passed[i] = 0;
+
+			} else {
+				printf("  [%s] \t\tPASS\n", microbenchmarks[i].name);
+				fflush(stdout);
+				passed[i] = 1;
+			}
+
+			for (int j = 0; j < 80; j++) {
+				printf("-");
+			}
+
+			testcount++;
+			printf("\n\n");
+		}
+	}
+
+	for (size_t k = 0; k < i; k++) {
+		if (!passed[k] && !(microbenchmarks[k].options & option)) {
+			printf(" [%s] to obtain details, please re-run with\n\t nsh> microbench %s\n\n", microbenchmarks[k].name,
+			       microbenchmarks[k].name);
+		}
+	}
+
+	return 0;
+}
+
+extern "C" __EXPORT int microbench_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		PX4_WARN("missing test name - 'microbench help' for a list of tests");
+		return 1;
+	}
+
+	for (size_t i = 0; microbenchmarks[i].name; i++) {
+		if (!strcmp(microbenchmarks[i].name, argv[1])) {
+			if (microbenchmarks[i].fn(argc - 1, argv + 1) == 0) {
+				PX4_INFO("%s PASSED", microbenchmarks[i].name);
+				return 0;
+
+			} else {
+				PX4_ERR("%s FAILED", microbenchmarks[i].name);
+				return -1;
+			}
+		}
+	}
+
+	PX4_WARN("no test called '%s' - 'microbench help' for a list of tests", argv[1]);
+	return 1;
+}

src/systemcmds/microbench/test_microbench_atomic.cpp

@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2020-2021 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_microbench_atomic.cpp
+ * Microbenchmark atomic operations.
+ */
+
+#include <unit_test.h>
+
+#include <time.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/micro_hal.h>
+#include <px4_platform_common/atomic.h>
+
+#ifdef __PX4_NUTTX
+#include <nuttx/irq.h>
+#endif
+
+namespace MicroBenchAtomic
+{
+
+#define PERF(name, op, count) do { \
+		px4_usleep(100); \
+		reset(); \
+		perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
+		for (int i = 0; i < count; i++) { \
+			px4_usleep(1); \
+			lock(); \
+			perf_begin(p); \
+			op; \
+			perf_end(p); \
+			unlock(); \
+			reset(); \
+		} \
+		perf_print_counter(p); \
+		perf_free(p); \
+	} while (0)
+
+class MicroBenchAtomic : public UnitTest
+{
+public:
+	bool run_tests() override;
+
+private:
+
+	bool time_atomic_bool();
+	bool time_atomic_int8();
+	bool time_atomic_int16();
+	bool time_atomic_int32();
+	bool time_atomic_uint32();
+	bool time_atomic_float();
+	bool time_atomic_hrt_abstime();
+
+	void reset();
+
+	void lock()
+	{
+#ifdef __PX4_NUTTX
+		_flags = px4_enter_critical_section();
+#endif
+	}
+
+	void unlock()
+	{
+#ifdef __PX4_NUTTX
+		px4_leave_critical_section(_flags);
+#endif
+	}
+
+#ifdef __PX4_NUTTX
+	irqstate_t _flags {};
+#endif
+
+	px4::atomic<bool> _atomic_bool{false};
+	px4::atomic<bool> _atomic_bool_storage{false};
+	bool _test_load_bool{};
+
+	px4::atomic<int8_t> _atomic_int8{0};
+	px4::atomic<int8_t> _atomic_int8_storage{0};
+	int8_t _test_load_int8{};
+
+	px4::atomic<int16_t> _atomic_int16{0};
+	px4::atomic<int16_t> _atomic_int16_storage{0};
+	int16_t _test_load_int16{};
+
+	px4::atomic<int32_t> _atomic_int32{0};
+	px4::atomic<int32_t> _atomic_int32_storage{0};
+	int32_t _test_load_int32{};
+
+	px4::atomic<uint32_t> _atomic_uint32{0};
+	px4::atomic<uint32_t> _atomic_uint32_storage{0};
+	uint32_t _test_load_uint32{};
+
+	px4::atomic<float> _atomic_float{0};
+	px4::atomic<float> _atomic_float_storage{0};
+	float _test_load_float{};
+
+	px4::atomic<hrt_abstime> _atomic_hrt_abstime{0};
+	px4::atomic<hrt_abstime> _atomic_hrt_abstime_storage{0};
+	hrt_abstime _test_load_hrt_abstime{};
+};
+
+bool MicroBenchAtomic::run_tests()
+{
+	ut_run_test(time_atomic_bool);
+	ut_run_test(time_atomic_int8);
+	ut_run_test(time_atomic_int16);
+	ut_run_test(time_atomic_int32);
+	ut_run_test(time_atomic_uint32);
+	ut_run_test(time_atomic_float);
+	ut_run_test(time_atomic_hrt_abstime);
+
+	return (_tests_failed == 0);
+}
+
+void MicroBenchAtomic::reset()
+{
+	srand(time(nullptr));
+
+	_atomic_bool.store(rand());
+	_atomic_bool_storage.store(rand());
+	_test_load_bool = rand();
+
+	_atomic_int8.store(rand());
+	_atomic_int8_storage.store(rand());
+	_test_load_int8 = rand();
+
+	_atomic_int16.store(rand());
+	_atomic_int16_storage.store(rand());
+	_test_load_int16 = rand();
+
+	_atomic_int32.store(rand());
+	_atomic_int32_storage.store(rand());
+	_test_load_int32 = rand();
+
+	_atomic_uint32.store(rand());
+	_atomic_uint32_storage.store(rand());
+	_test_load_uint32 = rand();
+
+	_atomic_float.store(rand());
+	_atomic_float_storage.store(rand());
+	_test_load_float = rand();
+}
+
+ut_declare_test_c(test_microbench_atomic, MicroBenchAtomic)
+
+bool MicroBenchAtomic::time_atomic_bool()
+{
+	PERF("atomic bool load", _test_load_bool = _atomic_bool.load(), 100);
+	PERF("atomic bool store", _atomic_bool.store(_test_load_bool), 100);
+	PERF("atomic bool load and store", _atomic_bool_storage.store(_atomic_bool.load()), 100);
+
+	bool expected = true;
+	PERF("atomic bool compare exchange (same)",
+	     volatile bool compare_exchange = _atomic_bool.compare_exchange(&expected, true), 100);
+	PERF("atomic bool compare exchange (different)",
+	     volatile bool compare_exchange = _atomic_bool.compare_exchange(&expected, false), 100);
+
+	return true;
+}
+
+bool MicroBenchAtomic::time_atomic_int8()
+{
+	PERF("atomic int8 load", _test_load_int8 = _atomic_int8.load(), 100);
+	PERF("atomic int8 store", _atomic_int8.store(_test_load_int8), 100);
+	PERF("atomic int8 load and store", _atomic_int8_storage.store(_atomic_int8.load()), 100);
+	PERF("atomic int8 fetch add", _test_load_int8 = _atomic_int8.fetch_add(_test_load_int8), 100);
+	PERF("atomic int8 fetch sub", _test_load_int8 = _atomic_int8.fetch_sub(_test_load_int8), 100);
+	PERF("atomic int8 fetch and", _test_load_int8 = _atomic_int8.fetch_and(_test_load_int8), 100);
+	PERF("atomic int8 fetch xor", _test_load_int8 = _atomic_int8.fetch_xor(_test_load_int8), 100);
+	PERF("atomic int8 fetch or", _test_load_int8 = _atomic_int8.fetch_or(_test_load_int8), 100);
+	PERF("atomic int8 fetch nand", _test_load_int8 = _atomic_int8.fetch_nand(_test_load_int8), 100);
+
+	int8_t expected = 42;
+	PERF("atomic int8 compare exchange (same)",
+	     volatile bool compare_exchange = _atomic_int8.compare_exchange(&expected, 42), 100);
+	PERF("atomic int8 compare exchange (different)",
+	     volatile bool compare_exchange = _atomic_int8.compare_exchange(&expected, 0), 100);
+
+	return true;
+}
+
+bool MicroBenchAtomic::time_atomic_int16()
+{
+	PERF("atomic int16 load", _test_load_int16 = _atomic_int16.load(), 100);
+	PERF("atomic int16 store", _atomic_int16.store(_test_load_int16), 100);
+	PERF("atomic int16 load and store", _atomic_int16_storage.store(_atomic_int16.load()), 100);
+	PERF("atomic int16 fetch add", _test_load_int16 = _atomic_int16.fetch_add(_test_load_int16), 100);
+	PERF("atomic int16 fetch sub", _test_load_int16 = _atomic_int16.fetch_sub(_test_load_int16), 100);
+	PERF("atomic int16 fetch and", _test_load_int16 = _atomic_int16.fetch_and(_test_load_int16), 100);
+	PERF("atomic int16 fetch xor", _test_load_int16 = _atomic_int16.fetch_xor(_test_load_int16), 100);
+	PERF("atomic int16 fetch or", _test_load_int16 = _atomic_int16.fetch_or(_test_load_int16), 100);
+	PERF("atomic int16 fetch nand", _test_load_int16 = _atomic_int16.fetch_nand(_test_load_int16), 100);
+
+	int16_t expected = 42;
+	PERF("atomic int16 compare exchange (same)",
+	     volatile bool compare_exchange = _atomic_int16.compare_exchange(&expected, 42), 100);
+	PERF("atomic int16 compare exchange (different)",
+	     volatile bool compare_exchange = _atomic_int16.compare_exchange(&expected, 0), 100);
+
+	return true;
+}
+
+bool MicroBenchAtomic::time_atomic_int32()
+{
+	PERF("atomic int32 load", _test_load_int32 = _atomic_int32.load(), 100);
+	PERF("atomic int32 store", _atomic_int32.store(_test_load_int32), 100);
+	PERF("atomic int32 load and store", _atomic_int32_storage.store(_atomic_int32.load()), 100);
+	PERF("atomic int32 fetch add", _test_load_int32 = _atomic_int32.fetch_add(_test_load_int32), 100);
+	PERF("atomic int32 fetch sub", _test_load_int32 = _atomic_int32.fetch_sub(_test_load_int32), 100);
+	PERF("atomic int32 fetch and", _test_load_int32 = _atomic_int32.fetch_and(_test_load_int32), 100);
+	PERF("atomic int32 fetch xor", _test_load_int32 = _atomic_int32.fetch_xor(_test_load_int32), 100);
+	PERF("atomic int32 fetch or", _test_load_int32 = _atomic_int32.fetch_or(_test_load_int32), 100);
+	PERF("atomic int32 fetch nand", _test_load_int32 = _atomic_int32.fetch_nand(_test_load_int32), 100);
+
+	int32_t expected = 42;
+	PERF("atomic int32 compare exchange (same)",
+	     volatile bool compare_exchange = _atomic_int32.compare_exchange(&expected, 42), 100);
+	PERF("atomic int32 compare exchange (different)",
+	     volatile bool compare_exchange = _atomic_int32.compare_exchange(&expected, 0), 100);
+
+	return true;
+}
+
+bool MicroBenchAtomic::time_atomic_uint32()
+{
+	PERF("atomic uint32 load", _test_load_uint32 = _atomic_uint32.load(), 100);
+	PERF("atomic uint32 store", _atomic_uint32_storage.store(_test_load_uint32), 100);
+	PERF("atomic uint32 load and store", _atomic_uint32_storage.store(_atomic_uint32.load()), 100);
+	PERF("atomic uint32 fetch add", _test_load_uint32 = _atomic_uint32.fetch_add(_test_load_uint32), 100);
+	PERF("atomic uint32 fetch sub", _test_load_uint32 = _atomic_uint32.fetch_sub(_test_load_uint32), 100);
+	PERF("atomic uint32 fetch and", _test_load_uint32 = _atomic_uint32.fetch_and(_test_load_uint32), 100);
+	PERF("atomic uint32 fetch xor", _test_load_uint32 = _atomic_uint32.fetch_xor(_test_load_uint32), 100);
+	PERF("atomic uint32 fetch or", _test_load_uint32 = _atomic_uint32.fetch_or(_test_load_uint32), 100);
+	PERF("atomic uint32 fetch nand", _test_load_uint32 = _atomic_uint32.fetch_nand(_test_load_uint32), 100);
+
+	uint32_t expected = 42;
+	PERF("atomic uint32 compare exchange (same)",
+	     volatile bool compare_exchange = _atomic_uint32.compare_exchange(&expected, 42), 100);
+	PERF("atomic uint32 compare exchange (different)",
+	     volatile bool compare_exchange = _atomic_uint32.compare_exchange(&expected, 0), 100);
+
+	return true;
+}
+
+bool MicroBenchAtomic::time_atomic_float()
+{
+	//PERF("atomic float load", volatile float test_load = _atomic_float.load(), 100);
+	PERF("atomic float store", _atomic_float.store(_test_load_float), 100);
+	//PERF("atomic float load and store", _atomic_float_storage.store(_atomic_float.load()), 100);
+
+	float expected = 42;
+	PERF("atomic float compare exchange (same)",
+	     volatile bool compare_exchange = _atomic_float.compare_exchange(&expected, 42), 100);
+	PERF("atomic float compare exchange (different)",
+	     volatile bool compare_exchange = _atomic_float.compare_exchange(&expected, 0), 100);
+
+	return true;
+}
+
+bool MicroBenchAtomic::time_atomic_hrt_abstime()
+{
+	ut_compare("atomic hrt_abstime load", _atomic_hrt_abstime.load(), 0);
+	PERF("atomic hrt_abstime load", volatile hrt_abstime test_load = _atomic_hrt_abstime.load(), 100);
+
+	_test_load_hrt_abstime = 1;
+	PERF("atomic hrt_abstime store", _atomic_hrt_abstime.store(_test_load_hrt_abstime), 100);
+	ut_compare("atomic hrt_abstime load", _atomic_hrt_abstime.load(), 1);
+
+	PERF("atomic hrt_abstime load and store", _atomic_hrt_abstime_storage.store(_atomic_hrt_abstime.load()), 100);
+
+	hrt_abstime expected = 12345678;
+	PERF("atomic hrt_abstime compare exchange (same)",
+	     volatile bool compare_exchange = _atomic_hrt_abstime.compare_exchange(&expected, 12345678), 100);
+	ut_compare("atomic hrt_abstime load", _atomic_hrt_abstime.compare_exchange(&expected, 12345678) == true, true);
+
+	PERF("atomic hrt_abstime compare exchange (different)",
+	     volatile bool compare_exchange = _atomic_hrt_abstime.compare_exchange(&expected, 0), 100);
+	ut_compare("atomic hrt_abstime load", _atomic_hrt_abstime.compare_exchange(&expected, 0) == false, false);
+
+	return true;
+}
+
+} // namespace MicroBenchAtomic

src/systemcmds/microbench/test_microbench_hrt.cpp

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2018-2021 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_microbench_hrt.cpp
+ * Tests for the microbench high resolution timer.
+ */
+
+#include <unit_test.h>
+
+#include <time.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/micro_hal.h>
+
+namespace MicroBenchHRT
+{
+
+#ifdef __PX4_NUTTX
+#include <nuttx/irq.h>
+static irqstate_t flags;
+#endif
+
+void lock()
+{
+#ifdef __PX4_NUTTX
+	flags = px4_enter_critical_section();
+#endif
+}
+
+void unlock()
+{
+#ifdef __PX4_NUTTX
+	px4_leave_critical_section(flags);
+#endif
+}
+
+#define PERF(name, op, count) do { \
+		px4_usleep(1000); \
+		reset(); \
+		perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
+		for (int i = 0; i < count; i++) { \
+			px4_usleep(1); \
+			lock(); \
+			perf_begin(p); \
+			op; \
+			perf_end(p); \
+			unlock(); \
+			reset(); \
+		} \
+		perf_print_counter(p); \
+		perf_free(p); \
+	} while (0)
+
+class MicroBenchHRT : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+private:
+
+	bool time_px4_hrt();
+
+	void reset();
+
+	void lock()
+	{
+#ifdef __PX4_NUTTX
+		flags = px4_enter_critical_section();
+#endif
+	}
+
+	void unlock()
+	{
+#ifdef __PX4_NUTTX
+		px4_leave_critical_section(flags);
+#endif
+	}
+
+	uint64_t u_64;
+	uint64_t u_64_out;
+};
+
+bool MicroBenchHRT::run_tests()
+{
+	ut_run_test(time_px4_hrt);
+
+	return (_tests_failed == 0);
+}
+
+template<typename T>
+T random(T min, T max)
+{
+	const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */
+	return min + scale * (max - min);      /* [min, max] */
+}
+
+void MicroBenchHRT::reset()
+{
+	srand(time(nullptr));
+
+	// initialize with random data
+	u_64 = rand();
+	u_64_out = rand();
+}
+
+ut_declare_test_c(test_microbench_hrt, MicroBenchHRT)
+
+bool MicroBenchHRT::time_px4_hrt()
+{
+	PERF("hrt_absolute_time()", u_64_out = hrt_absolute_time(), 1000);
+	PERF("hrt_elapsed_time()", u_64_out = hrt_elapsed_time(&u_64), 1000);
+	PERF("hrt_elapsed_time_atomic()", u_64_out = hrt_elapsed_time_atomic(&u_64), 1000);
+
+	return true;
+}
+
+} // namespace MicroBenchHRT

src/systemcmds/microbench/test_microbench_math.cpp

@@ -0,0 +1,272 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2018-2021 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_microbench_math.cpp
+ * Tests for the microbench math library.
+ */
+
+#include <unit_test.h>
+
+#include <time.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/micro_hal.h>
+
+namespace MicroBenchMath
+{
+
+#ifdef __PX4_NUTTX
+#include <nuttx/irq.h>
+static irqstate_t flags;
+#endif
+
+void lock()
+{
+#ifdef __PX4_NUTTX
+	flags = px4_enter_critical_section();
+#endif
+}
+
+void unlock()
+{
+#ifdef __PX4_NUTTX
+	px4_leave_critical_section(flags);
+#endif
+}
+
+#define PERF(name, op, count) do { \
+		px4_usleep(1000); \
+		reset(); \
+		perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
+		for (int i = 0; i < count; i++) { \
+			px4_usleep(1); \
+			lock(); \
+			perf_begin(p); \
+			op; \
+			perf_end(p); \
+			unlock(); \
+			reset(); \
+		} \
+		perf_print_counter(p); \
+		perf_free(p); \
+	} while (0)
+
+class MicroBenchMath : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+private:
+	bool time_single_precision_float();
+	bool time_single_precision_float_trig();
+
+	bool time_double_precision_float();
+	bool time_double_precision_float_trig();
+
+	bool time_8bit_integers();
+	bool time_16bit_integers();
+	bool time_32bit_integers();
+	bool time_64bit_integers();
+
+	void reset();
+
+	volatile float f32;
+	volatile float f32_out;
+
+	volatile double f64;
+	volatile double f64_out;
+
+	volatile uint8_t i_8;
+	volatile uint8_t i_8_out;
+
+	volatile uint16_t i_16;
+	volatile uint16_t i_16_out;
+
+	volatile uint32_t i_32;
+	volatile uint32_t i_32_out;
+
+	volatile int64_t i_64;
+	volatile int64_t i_64_out;
+
+	volatile uint64_t u_64;
+	volatile uint64_t u_64_out;
+};
+
+bool MicroBenchMath::run_tests()
+{
+	ut_run_test(time_single_precision_float);
+	ut_run_test(time_single_precision_float_trig);
+	ut_run_test(time_double_precision_float);
+	ut_run_test(time_double_precision_float_trig);
+	ut_run_test(time_8bit_integers);
+	ut_run_test(time_16bit_integers);
+	ut_run_test(time_32bit_integers);
+	ut_run_test(time_64bit_integers);
+
+	return (_tests_failed == 0);
+}
+
+template<typename T>
+T random(T min, T max)
+{
+	const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */
+	return min + scale * (max - min);      /* [min, max] */
+}
+
+void MicroBenchMath::reset()
+{
+	srand(time(nullptr));
+
+	// initialize with random data
+	f32 = random(-2.0f * M_PI, 2.0f * M_PI);		// somewhat representative range for angles in radians
+	f32_out = random(-2.0f * M_PI, 2.0f * M_PI);
+
+	f64 = random(-2.0 * M_PI, 2.0 * M_PI);
+	f64_out = random(-2.0 * M_PI, 2.0 * M_PI);
+
+	i_8 = rand();
+	i_8_out = rand();
+
+	i_16 = rand();
+	i_16_out = rand();
+
+	i_32 = rand();
+	i_32_out = rand();
+
+	i_64 = rand();
+	i_64_out = rand();
+
+	u_64 = rand();
+	u_64_out = rand();
+}
+
+ut_declare_test_c(test_microbench_math, MicroBenchMath)
+
+bool MicroBenchMath::time_single_precision_float()
+{
+	PERF("float add", f32_out += f32, 100);
+	PERF("float sub", f32_out -= f32, 100);
+	PERF("float mul", f32_out *= f32, 100);
+	PERF("float div", f32_out /= f32, 100);
+	PERF("float sqrt", f32_out = sqrtf(f32), 100);
+
+	return true;
+}
+
+bool MicroBenchMath::time_single_precision_float_trig()
+{
+	PERF("sinf()", f32_out = sinf(f32), 100);
+	PERF("cosf()", f32_out = cosf(f32), 100);
+	PERF("tanf()", f32_out = tanf(f32), 100);
+
+	PERF("acosf()", f32_out = acosf(f32), 100);
+	PERF("asinf()", f32_out = asinf(f32), 100);
+	PERF("atan2f()", f32_out = atan2f(f32, 2.0f * f32), 100);
+
+	return true;
+}
+
+bool MicroBenchMath::time_double_precision_float()
+{
+	PERF("double add", f64_out += f64, 100);
+	PERF("double sub", f64_out -= f64, 100);
+	PERF("double mul", f64_out *= f64, 100);
+	PERF("double div", f64_out /= f64, 100);
+	PERF("double sqrt", f64_out = sqrt(f64), 100);
+
+	return true;
+}
+
+bool MicroBenchMath::time_double_precision_float_trig()
+{
+	PERF("sin()", f64_out = sin(f64), 100);
+	PERF("cos()", f64_out = cos(f64), 100);
+	PERF("tan()", f64_out = tan(f64), 100);
+
+	PERF("acos()", f64_out = acos(f64 * 0.5), 100);
+	PERF("asin()", f64_out = asin(f64 * 0.6), 100);
+	PERF("atan2()", f64_out = atan2(f64 * 0.7, f64 * 0.8), 100);
+
+	PERF("sqrt()", f64_out = sqrt(f64), 100);
+
+	return true;
+}
+
+
+bool MicroBenchMath::time_8bit_integers()
+{
+	PERF("int8 add", i_8_out += i_8, 100);
+	PERF("int8 sub", i_8_out -= i_8, 100);
+	PERF("int8 mul", i_8_out *= i_8, 100);
+	PERF("int8 div", i_8_out /= i_8, 100);
+
+	return true;
+}
+
+bool MicroBenchMath::time_16bit_integers()
+{
+	PERF("int16 add", i_16_out += i_16, 100);
+	PERF("int16 sub", i_16_out -= i_16, 100);
+	PERF("int16 mul", i_16_out *= i_16, 100);
+	PERF("int16 div", i_16_out /= i_16, 100);
+
+	return true;
+}
+
+bool MicroBenchMath::time_32bit_integers()
+{
+	PERF("int32 add", i_32_out += i_32, 100);
+	PERF("int32 sub", i_32_out -= i_32, 100);
+	PERF("int32 mul", i_32_out *= i_32, 100);
+	PERF("int32 div", i_32_out /= i_32, 100);
+
+	return true;
+}
+
+bool MicroBenchMath::time_64bit_integers()
+{
+	PERF("int64 add", i_64_out += i_64, 100);
+	PERF("int64 sub", i_64_out -= i_64, 100);
+	PERF("int64 mul", i_64_out *= i_64, 100);
+	PERF("int64 div", i_64_out /= i_64, 100);
+
+	return true;
+}
+
+} // namespace MicroBenchMath

src/systemcmds/microbench/test_microbench_matrix.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2018-2021 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_microbench_matrix.cpp
+ * Tests for the microbench matrix math library.
+ */
+
+#include <unit_test.h>
+
+#include <time.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/micro_hal.h>
+
+#include <matrix/math.hpp>
+
+namespace MicroBenchMatrix
+{
+
+#ifdef __PX4_NUTTX
+#include <nuttx/irq.h>
+static irqstate_t flags;
+#endif
+
+void lock()
+{
+#ifdef __PX4_NUTTX
+	flags = px4_enter_critical_section();
+#endif
+}
+
+void unlock()
+{
+#ifdef __PX4_NUTTX
+	px4_leave_critical_section(flags);
+#endif
+}
+
+#define PERF(name, op, count) do { \
+		px4_usleep(1000); \
+		reset(); \
+		perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
+		for (int i = 0; i < count; i++) { \
+			px4_usleep(1); \
+			lock(); \
+			perf_begin(p); \
+			op; \
+			perf_end(p); \
+			unlock(); \
+			reset(); \
+		} \
+		perf_print_counter(p); \
+		perf_free(p); \
+	} while (0)
+
+class MicroBenchMatrix : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+private:
+
+	bool time_matrix_euler();
+	bool time_matrix_quaternion();
+	bool time_matrix_dcm();
+	bool time_matrix_pseduo_inverse();
+
+	void reset();
+
+	matrix::Quatf q;
+	matrix::Eulerf e;
+	matrix::Dcmf d;
+	matrix::Matrix<float, 16, 6> A16;
+	matrix::Matrix<float, 6, 16> B16;
+	matrix::Matrix<float, 6, 16> B16_4;
+};
+
+bool MicroBenchMatrix::run_tests()
+{
+	ut_run_test(time_matrix_euler);
+	ut_run_test(time_matrix_quaternion);
+	ut_run_test(time_matrix_dcm);
+	ut_run_test(time_matrix_pseduo_inverse);
+
+	return (_tests_failed == 0);
+}
+
+template<typename T>
+T random(T min, T max)
+{
+	const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */
+	return min + scale * (max - min);      /* [min, max] */
+}
+
+void MicroBenchMatrix::reset()
+{
+	srand(time(nullptr));
+
+	// initialize with random data
+	q = matrix::Quatf(rand(), rand(), rand(), rand());
+	e = matrix::Eulerf(random(-2.0 * M_PI, 2.0 * M_PI), random(-2.0 * M_PI, 2.0 * M_PI), random(-2.0 * M_PI, 2.0 * M_PI));
+	d = q;
+
+	for (size_t j = 0; j < 6; j++) {
+		for (size_t i = 0; i < 16; i++) {
+			B16(j, i) = random(-10.0, 10.0);
+		}
+
+		for (size_t i = 0; i < 4; i++) {
+			B16_4(j, i) = random(-10.0, 10.0);
+		}
+	}
+}
+
+bool MicroBenchMatrix::time_matrix_euler()
+{
+	PERF("matrix Euler from Quaternion", e = q, 100);
+	PERF("matrix Euler from Dcm", e = d, 100);
+	return true;
+}
+
+bool MicroBenchMatrix::time_matrix_quaternion()
+{
+	PERF("matrix Quaternion from Euler", q = e, 100);
+	PERF("matrix Quaternion from Dcm", q = d, 100);
+	return true;
+}
+
+bool MicroBenchMatrix::time_matrix_dcm()
+{
+	PERF("matrix Dcm from Euler", d = e, 100);
+	PERF("matrix Dcm from Quaternion", d = q, 100);
+	return true;
+}
+
+bool MicroBenchMatrix::time_matrix_pseduo_inverse()
+{
+	PERF("matrix 6x16 pseudo inverse (all non-zero columns)", A16 = matrix::geninv(B16), 100);
+	PERF("matrix 6x16 pseudo inverse (4 non-zero columns)", A16 = matrix::geninv(B16_4), 100);
+	return true;
+}
+
+ut_declare_test_c(test_microbench_matrix, MicroBenchMatrix)
+
+} // namespace MicroBenchMatrix

src/systemcmds/microbench/test_microbench_uorb.cpp

@@ -0,0 +1,249 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2018-2021 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_microbench_uorb.cpp
+ * Tests for microbench uORB functionality.
+ */
+
+#include <unit_test.h>
+
+#include <time.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/micro_hal.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_gyro_fifo.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+namespace MicroBenchORB
+{
+
+#ifdef __PX4_NUTTX
+#include <nuttx/irq.h>
+static irqstate_t flags;
+#endif
+
+void lock()
+{
+#ifdef __PX4_NUTTX
+	flags = px4_enter_critical_section();
+#endif
+}
+
+void unlock()
+{
+#ifdef __PX4_NUTTX
+	px4_leave_critical_section(flags);
+#endif
+}
+
+#define PERF(name, op, count) do { \
+		px4_usleep(1000); \
+		reset(); \
+		perf_counter_t p = perf_alloc(PC_ELAPSED, name); \
+		for (int i = 0; i < count; i++) { \
+			px4_usleep(1); \
+			lock(); \
+			perf_begin(p); \
+			op; \
+			perf_end(p); \
+			unlock(); \
+			reset(); \
+		} \
+		perf_print_counter(p); \
+		perf_free(p); \
+	} while (0)
+
+class MicroBenchORB : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+private:
+
+	bool time_px4_uorb();
+	bool time_px4_uorb_direct();
+
+	void reset();
+
+	vehicle_status_s status;
+	vehicle_local_position_s lpos;
+	sensor_gyro_s gyro;
+	sensor_gyro_fifo_s gyro_fifo;
+};
+
+bool MicroBenchORB::run_tests()
+{
+	ut_run_test(time_px4_uorb);
+	ut_run_test(time_px4_uorb_direct);
+
+	return (_tests_failed == 0);
+}
+
+template<typename T>
+T random(T min, T max)
+{
+	const T scale = rand() / (T) RAND_MAX; /* [0, 1.0] */
+	return min + scale * (max - min);      /* [min, max] */
+}
+
+void MicroBenchORB::reset()
+{
+	srand(time(nullptr));
+
+	// initialize with random data
+	status.timestamp = rand();
+	status.mission_failure = rand();
+
+	lpos.timestamp = rand();
+	lpos.dist_bottom_valid = rand();
+
+	gyro.timestamp = rand();
+
+	gyro_fifo.timestamp = rand();
+}
+
+ut_declare_test_c(test_microbench_uorb, MicroBenchORB)
+
+bool MicroBenchORB::time_px4_uorb()
+{
+	int fd_status = orb_subscribe(ORB_ID(vehicle_status));
+	int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
+	int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+	int fd_gyro_fifo = orb_subscribe(ORB_ID(sensor_gyro_fifo));
+
+	int ret = 0;
+	bool updated = false;
+
+	PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 100);
+	PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 100);
+
+	printf("\n");
+
+	PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 100);
+	PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 100);
+
+	printf("\n");
+
+	PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 100);
+	PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &gyro), 100);
+
+	printf("\n");
+
+	PERF("orb_check sensor_gyro_fifo", ret = orb_check(fd_gyro_fifo, &updated), 100);
+	PERF("orb_copy sensor_gyro_fifo", ret = orb_copy(ORB_ID(sensor_gyro_fifo), fd_gyro_fifo, &gyro_fifo), 100);
+
+	printf("\n");
+
+	PERF("orb_exists sensor_accel 0", ret = orb_exists(ORB_ID(sensor_accel), 0), 100);
+	PERF("orb_exists sensor_accel 1", ret = orb_exists(ORB_ID(sensor_accel), 1), 100);
+	PERF("orb_exists sensor_accel 2", ret = orb_exists(ORB_ID(sensor_accel), 2), 100);
+	PERF("orb_exists sensor_accel 3", ret = orb_exists(ORB_ID(sensor_accel), 3), 100);
+	PERF("orb_exists sensor_accel 4", ret = orb_exists(ORB_ID(sensor_accel), 4), 100);
+	PERF("orb_exists sensor_accel 5", ret = orb_exists(ORB_ID(sensor_accel), 5), 100);
+	PERF("orb_exists sensor_accel 6", ret = orb_exists(ORB_ID(sensor_accel), 6), 100);
+	PERF("orb_exists sensor_accel 7", ret = orb_exists(ORB_ID(sensor_accel), 7), 100);
+	PERF("orb_exists sensor_accel 8", ret = orb_exists(ORB_ID(sensor_accel), 8), 100);
+	PERF("orb_exists sensor_accel 9", ret = orb_exists(ORB_ID(sensor_accel), 9), 100);
+	PERF("orb_exists sensor_accel 10", ret = orb_exists(ORB_ID(sensor_accel), 10), 100);
+
+	orb_unsubscribe(fd_status);
+	orb_unsubscribe(fd_lpos);
+	orb_unsubscribe(fd_gyro);
+	orb_unsubscribe(fd_gyro_fifo);
+
+	return true;
+}
+
+bool MicroBenchORB::time_px4_uorb_direct()
+{
+	bool ret = false;
+
+	uORB::Subscription vstatus{ORB_ID(vehicle_status)};
+	PERF("uORB::Subscription orb_check vehicle_status", ret = vstatus.updated(), 100);
+	PERF("uORB::Subscription orb_copy vehicle_status", ret = vstatus.copy(&status), 100);
+
+	printf("\n");
+
+	uORB::Subscription local_pos{ORB_ID(vehicle_local_position)};
+	PERF("uORB::Subscription orb_check vehicle_local_position", ret = local_pos.updated(), 100);
+	PERF("uORB::Subscription orb_copy vehicle_local_position", ret = local_pos.copy(&lpos), 100);
+
+	{
+		printf("\n");
+		uORB::Subscription sens_gyro0{ORB_ID(sensor_gyro), 0};
+		PERF("uORB::Subscription orb_check sensor_gyro:0", ret = sens_gyro0.updated(), 100);
+		PERF("uORB::Subscription orb_copy sensor_gyro:0", ret = sens_gyro0.copy(&gyro), 100);
+	}
+
+	{
+		printf("\n");
+		uORB::Subscription sens_gyro1{ORB_ID(sensor_gyro), 1};
+		PERF("uORB::Subscription orb_check sensor_gyro:1", ret = sens_gyro1.updated(), 100);
+		PERF("uORB::Subscription orb_copy sensor_gyro:1", ret = sens_gyro1.copy(&gyro), 100);
+	}
+
+	{
+		printf("\n");
+		uORB::Subscription sens_gyro2{ORB_ID(sensor_gyro), 2};
+		PERF("uORB::Subscription orb_check sensor_gyro:2", ret = sens_gyro2.updated(), 100);
+		PERF("uORB::Subscription orb_copy sensor_gyro:2", ret = sens_gyro2.copy(&gyro), 100);
+	}
+
+	{
+		printf("\n");
+		uORB::Subscription sens_gyro3{ORB_ID(sensor_gyro), 3};
+		PERF("uORB::Subscription orb_check sensor_gyro:3", ret = sens_gyro3.updated(), 100);
+		PERF("uORB::Subscription orb_copy sensor_gyro:3", ret = sens_gyro3.copy(&gyro), 100);
+	}
+
+	{
+		printf("\n");
+		uORB::Subscription sens_gyro_fifo0{ORB_ID(sensor_gyro_fifo), 0};
+		PERF("uORB::Subscription orb_check sensor_gyro_fifo:0", ret = sens_gyro_fifo0.updated(), 100);
+		PERF("uORB::Subscription orb_copy sensor_gyro_fifo:0", ret = sens_gyro_fifo0.copy(&gyro_fifo), 100);
+	}
+
+	return true;
+}
+
+} // namespace MicroBenchORB