diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
index 56482d1407..bc248ac04c 100644
--- a/ROMFS/px4fmu_test/init.d/rcS
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ]
 then
 	tests mount
 fi
+
+#
+# Run unit tests at board boot, reporting failure as needed.
+# Add new unit tests using the same pattern as below.
+#
+
+set unit_test_failure 0
+
+if mavlink_tests
+then
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
+fi
+
+if commander_tests
+then
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
+fi
+
+if [ $unit_test_failure == 0 ]
+then
+	echo
+	echo "All Unit Tests PASSED"
+else
+	echo
+	echo "Some Unit Tests FAILED:${unit_test_failure_list}"
+fi
\ No newline at end of file
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index a41670a77b..2f4d9d6a47 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -56,6 +56,7 @@ LIBRARIES	+= lib/mathlib/CMSIS
 
 MODULES 	+= modules/unit_test
 MODULES		+= modules/mavlink/mavlink_tests
+MODULES 	+= modules/commander/commander_tests
 
 #
 # Transitional support - add commands from the NuttX export archive.
diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
index 0abb84a82c..2bfa5d0bb1 100644
--- a/src/modules/commander/commander_tests/commander_tests.cpp
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
 
 int commander_tests_main(int argc, char *argv[])
 {
-	stateMachineHelperTest();
-
-	return 0;
+	return stateMachineHelperTest() ? 0 : -1;
 }
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 08dda2fabf..874090e93e 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -49,7 +49,7 @@ public:
 	StateMachineHelperTest();
 	virtual ~StateMachineHelperTest();
 
-	virtual void runTests(void);
+	virtual bool run_tests(void);
 
 private:
 	bool armingStateTransitionTest();
@@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void)
 	return true;
 }
 
-void StateMachineHelperTest::runTests(void)
+bool StateMachineHelperTest::run_tests(void)
 {
 	ut_run_test(armingStateTransitionTest);
 	ut_run_test(mainStateTransitionTest);
 	ut_run_test(isSafeTest);
+	
+	return (_tests_failed == 0);
 }
 
-void stateMachineHelperTest(void)
-{
-	StateMachineHelperTest* test = new StateMachineHelperTest();
-    test->runTests();
-	test->printResults();
-}
+ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
\ No newline at end of file
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
index bbf66255ea..cf67190955 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.h
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -39,6 +39,6 @@
 #ifndef STATE_MACHINE_HELPER_TEST_H_
 #define STATE_MACHINE_HELPER_TEST_
 
-void stateMachineHelperTest(void);
+bool stateMachineHelperTest(void);
 
 #endif /* STATE_MACHINE_HELPER_TEST_H_ */
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
index 022041c74b..a5fb2117ea 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
@@ -65,7 +65,7 @@ MavlinkFtpTest::~MavlinkFtpTest()
 }
 
 /// @brief Called before every test to initialize the FTP Server.
-void MavlinkFtpTest::init(void)
+void MavlinkFtpTest::_init(void)
 {
 	_ftp_server = new MavlinkFTP;;
 	_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
@@ -74,7 +74,7 @@ void MavlinkFtpTest::init(void)
 }
 
 /// @brief Called after every test to take down the FTP Server.
-void MavlinkFtpTest::cleanup(void)
+void MavlinkFtpTest::_cleanup(void)
 {
 	delete _ftp_server;
 	
@@ -738,7 +738,7 @@ void MavlinkFtpTest::_cleanup_microsd(void)
 }
 
 /// @brief Runs all the unit tests
-void MavlinkFtpTest::runTests(void)
+bool MavlinkFtpTest::run_tests(void)
 {
 	ut_run_test(_ack_test);
 	ut_run_test(_bad_opcode_test);
@@ -753,5 +753,9 @@ void MavlinkFtpTest::runTests(void)
 	ut_run_test(_removedirectory_test);
 	ut_run_test(_createdirectory_test);
 	ut_run_test(_removefile_test);
+	
+	return (_tests_failed == 0);
+
 }
 
+ut_declare_test(mavlink_ftp_test, MavlinkFtpTest)
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
index babd909dac..2696192cc6 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
@@ -46,10 +46,7 @@ public:
 	MavlinkFtpTest();
 	virtual ~MavlinkFtpTest();
 	
-	virtual void init(void);
-	virtual void cleanup(void);
-	
-	virtual void runTests(void);
+	virtual bool run_tests(void);
 	
 	static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
 	
@@ -65,6 +62,9 @@ public:
 	MavlinkFtpTest& operator=(const MavlinkFtpTest&);
 	
 private:
+	virtual void _init(void);
+	virtual void _cleanup(void);
+	
 	bool _ack_test(void);
 	bool _bad_opcode_test(void);
 	bool _bad_datasize_test(void);
@@ -105,3 +105,4 @@ private:
 	static const char _unittest_microsd_file[];
 };
 
+bool mavlink_ftp_test(void);
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
index d9c1e413a5..10cbcb0ecc 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
+++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
@@ -43,10 +43,5 @@ extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
 
 int mavlink_tests_main(int argc, char *argv[])
 {
-	MavlinkFtpTest* test = new MavlinkFtpTest;
-	
-	test->runTests();
-	test->printResults();
-
-	return 0;
+	return mavlink_ftp_test() ? 0 : -1;
 }
diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk
index 07cfce1dc9..1cc28cce15 100644
--- a/src/modules/mavlink/mavlink_tests/module.mk
+++ b/src/modules/mavlink/mavlink_tests/module.mk
@@ -45,4 +45,6 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MODULE_STACKSIZE = 5000
 
+MAXOPTIMIZATION	 = -Os
+
 EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
index be3b9461a8..b7568ce3a4 100644
--- a/src/modules/unit_test/unit_test.cpp
+++ b/src/modules/unit_test/unit_test.cpp
@@ -36,7 +36,11 @@
 
 #include <systemlib/err.h>
 
-UnitTest::UnitTest()
+UnitTest::UnitTest() :
+	_tests_run(0),
+	_tests_failed(0),
+	_tests_passed(0),
+	_assertions(0)
 {
 }
 
@@ -44,20 +48,22 @@ UnitTest::~UnitTest()
 {
 }
 
-void UnitTest::printResults(void)
+void UnitTest::print_results(void)
 {
-    warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
-    warnx("  Tests passed : %d", mu_tests_passed());
-    warnx("  Tests failed : %d", mu_tests_failed());
-    warnx("  Assertions : %d", mu_assertion());
+	warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
+	warnx("  Tests passed : %d", _tests_passed);
+	warnx("  Tests failed : %d", _tests_failed);
+	warnx("  Assertions : %d", _assertions);
 }
 
-void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line)
+/// @brief Used internally to the ut_assert macro to print assert failures.
+void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line)
 {
-    warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
+	warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
 }
 
-void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
+/// @brief Used internally to the ut_compare macro to print assert failures.
+void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
 {
 	warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
 }
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index 1a54897094..8ed4efadf9 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -37,68 +37,85 @@
 
 #include <systemlib/err.h>
 
+/// @brief Base class to be used for unit tests.
 class __EXPORT UnitTest
 {
-
 public:
-#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
-
-INLINE_GLOBAL(int, mu_tests_run)
-INLINE_GLOBAL(int, mu_tests_failed)
-INLINE_GLOBAL(int, mu_tests_passed)
-INLINE_GLOBAL(int, mu_assertion)
-INLINE_GLOBAL(int, mu_line)
-INLINE_GLOBAL(const char*, mu_last_test)
 
 	UnitTest();
-    virtual ~UnitTest();
+	virtual ~UnitTest();
 	
-	virtual void init(void) { };
-	virtual void cleanup(void) { };
-
-    virtual void runTests(void) = 0;
-    void printResults(void);
-    
-    void printAssert(const char* msg, const char* test, const char* file, int line);
-	void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
-    
-#define ut_assert(message, test)                                \
-    do {                                                        \
-        if (!(test)) {                                          \
-            printAssert(message, #test, __FILE__, __LINE__);    \
-            return false;                                       \
-        } else {                                                \
-            mu_assertion()++;                                   \
-        }                                                       \
-    } while (0)
-    
-#define ut_compare(message, v1, v2)							\
-	do {										\
-		int _v1 = v1;								\
-		int _v2 = v2;								\
-		if (_v1 != _v2) {							\
-			printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__);	\
-			return false;							\
-		} else {								\
-			mu_assertion()++;						\
-		}									\
+	/// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro.
+	/// @return true: all unit tests succeeded, false: one or more unit tests failed
+	virtual bool run_tests(void) = 0;
+	
+	/// @brief Prints results from running of unit tests.
+	void print_results(void);
+	
+/// @brief Macro to create a function which will run a unit test class and print results.
+#define ut_declare_test(test_function, test_class)	\
+	bool test_function(void)			\
+	{						\
+		test_class* test = new test_class();	\
+		bool success = test->run_tests();	\
+		test->print_results();			\
+		return success;				\
+	}
+	
+protected:
+	
+/// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit
+/// test should return true if it succeeded, false for fail.
+#define ut_run_test(test)					\
+	do {							\
+		warnx("RUNNING TEST: %s", #test);		\
+		_tests_run++;					\
+		_init();						\
+		if (!test()) {					\
+			warnx("TEST FAILED: %s", #test);	\
+			_tests_failed++;			\
+		} else {					\
+			warnx("TEST PASSED: %s", #test);	\
+			_tests_passed++;			\
+		}						\
+		_cleanup();					\
 	} while (0)
-    
-#define ut_run_test(test)                       \
-    do {                                        \
-        warnx("RUNNING TEST: %s", #test);       \
-        mu_tests_run()++;                       \
-	init();					\
-        if (!test()) {                          \
-            warnx("TEST FAILED: %s", #test);    \
-            mu_tests_failed()++;                \
-        } else {                                \
-            warnx("TEST PASSED: %s", #test);    \
-            mu_tests_passed()++;                \
-        }                                       \
-	cleanup();				\
-    } while (0)
+	
+/// @brief Used to assert a value within a unit test.
+#define ut_assert(message, test)						\
+	do {									\
+		if (!(test)) {							\
+			_print_assert(message, #test, __FILE__, __LINE__);	\
+			return false;						\
+		} else {							\
+			_assertions++;						\
+		}								\
+	} while (0)
+	
+/// @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)								\
+	do {											\
+		int _v1 = v1;									\
+		int _v2 = v2;									\
+		if (_v1 != _v2) {								\
+			_print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__);	\
+			return false;								\
+		} else {									\
+			_assertions++;								\
+		}										\
+	} while (0)
+	
+	virtual void _init(void) { };		///< Run before each unit test. Override to provide custom behavior.
+	virtual void _cleanup(void) { };	///< Run after each unit test. Override to provide custom behavior.
+	
+	void _print_assert(const char* msg, const char* test, const char* file, int line);
+	void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
 
+	int _tests_run;		///< The number of individual unit tests run
+	int _tests_failed;	///< The number of unit tests which failed
+	int _tests_passed;	///< The number of unit tests which passed
+	int _assertions;	///< Total number of assertions tested by all unit tests
 };
 
 #endif /* UNIT_TEST_H_ */