Merge branch 'master' of github.com:PX4/Firmware into st24

17 files changed, 476 insertions, 171 deletions

diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
index 56482d140..bc248ac04 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 a41670a77..2f4d9d6a4 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 0abb84a82..2bfa5d0bb 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 08dda2fab..874090e93 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 bbf66255e..cf6719095 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/dataman/module.mk b/src/modules/dataman/module.mk
index 234607b3d..7ebe09fb7 100644
--- a/src/modules/dataman/module.mk
+++ b/src/modules/dataman/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= dataman
 SRCS		= dataman.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 2a85c3702..f17497aa8 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -34,6 +34,7 @@
 /// @file mavlink_ftp.cpp
 ///	@author px4dev, Don Gagne <don@thegagnes.com>
 
+#include <crc32.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
@@ -180,12 +181,16 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workList(payload);
 		break;
 
-	case kCmdOpenFile:
-		errorCode = _workOpen(payload, false);
+	case kCmdOpenFileRO:
+		errorCode = _workOpen(payload, O_RDONLY);
 		break;
 
 	case kCmdCreateFile:
-		errorCode = _workOpen(payload, true);
+		errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
+		break;
+
+	case kCmdOpenFileWO:
+		errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
 		break;
 
 	case kCmdReadFile:
@@ -200,6 +205,14 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workRemoveFile(payload);
 		break;
 
+	case kCmdRename:
+		errorCode = _workRename(payload);
+		break;
+
+	case kCmdTruncateFile:
+		errorCode = _workTruncateFile(payload);
+		break;
+
 	case kCmdCreateDirectory:
 		errorCode = _workCreateDirectory(payload);
 		break;
@@ -208,6 +221,11 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workRemoveDirectory(payload);
 		break;
 			
+
+	case kCmdCalcFileCRC32:
+		errorCode = _workCalcFileCRC32(payload);
+		break;
+
 	default:
 		errorCode = kErrUnknownCommand;
 		break;	
@@ -222,6 +240,7 @@ out:
 		warnx("FTP: ack\n");
 #endif
 	} else {
+		int r_errno = errno;
 		warnx("FTP: nak %u", errorCode);
 		payload->req_opcode = payload->opcode;
 		payload->opcode = kRspNak;
@@ -229,7 +248,7 @@ out:
 		payload->data[0] = errorCode;
 		if (errorCode == kErrFailErrno) {
 			payload->size = 2;
-			payload->data[1] = errno;
+			payload->data[1] = r_errno;
 		}
 	}
 
@@ -396,27 +415,27 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 
 /// @brief Responds to an Open command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
+MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
 {
 	int session_index = _find_unused_session();
 	if (session_index < 0) {
 		warnx("FTP: Open failed - out of sessions\n");
 		return kErrNoSessionsAvailable;
 	}
-	
+
 	char *filename = _data_as_cstring(payload);
-	
+
 	uint32_t fileSize = 0;
-	if (!create) {
-		struct stat st;
-		if (stat(filename, &st) != 0) {
+	struct stat st;
+	if (stat(filename, &st) != 0) {
+		// fail only if requested open for read
+		if (oflag & O_RDONLY)
 			return kErrFailErrno;
-		}
-		fileSize = st.st_size;
+		else
+			st.st_size = 0;
 	}
+	fileSize = st.st_size;
 
-	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
-    
 	int fd = ::open(filename, oflag);
 	if (fd < 0) {
 		return kErrFailErrno;
@@ -424,12 +443,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
 	_session_fds[session_index] = fd;
 
 	payload->session = session_index;
-	if (create) {
-		payload->size = 0;
-	} else {
-		payload->size = sizeof(uint32_t);
-		*((uint32_t*)payload->data) = fileSize;
-	}
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = fileSize;
 
 	return kErrNone;
 }
@@ -470,29 +485,33 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workWrite(PayloadHeader* payload)
 {
-#if 0
-    // NYI: Coming soon
-	auto hdr = req->header();
+	int session_index = payload->session;
 
-	// look up session
-	auto session = getSession(hdr->session);
-	if (session == nullptr) {
-		return kErrNoSession;
+	if (!_valid_session(session_index)) {
+		return kErrInvalidSession;
 	}
 
-	// append to file
-	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+	// Seek to the specified position
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("seek %d", payload->offset);
+#endif
+	if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
+		// Unable to see to the specified location
+		warnx("seek fail");
+		return kErrFailErrno;
+	}
 
-	if (result < 0) {
-		// XXX might also be no space, I/O, etc.
-		return kErrNotAppend;
+	int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
+	if (bytes_written < 0) {
+		// Negative return indicates error other than eof
+		warnx("write fail %d", bytes_written);
+		return kErrFailErrno;
 	}
 
-	hdr->size = result;
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = bytes_written;
+
 	return kErrNone;
-#else
-	return kErrUnknownCommand;
-#endif
 }
 
 /// @brief Responds to a RemoveFile command
@@ -510,6 +529,81 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
 	}
 }
 
+/// @brief Responds to a TruncateFile command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
+{
+	char file[kMaxDataLength];
+	const char temp_file[] = "/fs/microsd/.trunc.tmp";
+	strncpy(file, _data_as_cstring(payload), kMaxDataLength);
+	payload->size = 0;
+
+	// emulate truncate(file, payload->offset) by
+	// copying to temp and overwrite with O_TRUNC flag.
+
+	struct stat st;
+	if (stat(file, &st) != 0) {
+		return kErrFailErrno;
+	}
+
+	if (!S_ISREG(st.st_mode)) {
+		errno = EISDIR;
+		return kErrFailErrno;
+	}
+
+	// check perms allow us to write (not romfs)
+	if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) {
+		errno = EROFS;
+		return kErrFailErrno;
+	}
+
+	if (payload->offset == (unsigned)st.st_size) {
+		// nothing to do
+		return kErrNone;
+	}
+	else if (payload->offset == 0) {
+		// 1: truncate all data
+		int fd = ::open(file, O_TRUNC | O_WRONLY);
+		if (fd < 0) {
+			return kErrFailErrno;
+		}
+
+		::close(fd);
+		return kErrNone;
+	}
+	else if (payload->offset > (unsigned)st.st_size) {
+		// 2: extend file
+		int fd = ::open(file, O_WRONLY);
+		if (fd < 0) {
+			return kErrFailErrno;
+		}
+
+		if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) {
+			::close(fd);
+			return kErrFailErrno;
+		}
+
+		bool ok = 1 == ::write(fd, "", 1);
+		::close(fd);
+
+		return (ok)? kErrNone : kErrFailErrno;
+	}
+	else {
+		// 3: truncate
+		if (_copy_file(file, temp_file, payload->offset) != 0) {
+			return kErrFailErrno;
+		}
+		if (_copy_file(temp_file, file, payload->offset) != 0) {
+			return kErrFailErrno;
+		}
+		if (::unlink(temp_file) != 0) {
+			return kErrFailErrno;
+		}
+
+		return kErrNone;
+	}
+}
+
 /// @brief Responds to a Terminate command
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workTerminate(PayloadHeader* payload)
@@ -542,6 +636,33 @@ MavlinkFTP::_workReset(PayloadHeader* payload)
 	return kErrNone;
 }
 
+/// @brief Responds to a Rename command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRename(PayloadHeader* payload)
+{
+	char oldpath[kMaxDataLength];
+	char newpath[kMaxDataLength];
+
+	char *ptr = _data_as_cstring(payload);
+	size_t oldpath_sz = strlen(ptr);
+
+	if (oldpath_sz == payload->size) {
+		// no newpath
+		errno = EINVAL;
+		return kErrFailErrno;
+	}
+
+	strncpy(oldpath, ptr, kMaxDataLength);
+	strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength);
+
+	if (rename(oldpath, newpath) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
 /// @brief Responds to a RemoveDirectory command
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
@@ -572,6 +693,39 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
 	}
 }
 
+/// @brief Responds to a CalcFileCRC32 command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
+{
+	char file_buf[256];
+	uint32_t checksum = 0;
+	ssize_t bytes_read;
+	strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
+
+	int fd = ::open(file_buf, O_RDONLY);
+	if (fd < 0) {
+		return kErrFailErrno;
+	}
+
+	do {
+		bytes_read = ::read(fd, file_buf, sizeof(file_buf));
+		if (bytes_read < 0) {
+			int r_errno = errno;
+			::close(fd);
+			errno = r_errno;
+			return kErrFailErrno;
+		}
+
+		checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
+	} while (bytes_read == sizeof(file_buf));
+
+	::close(fd);
+
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = checksum;
+	return kErrNone;
+}
+
 /// @brief Returns true if the specified session is a valid open session
 bool
 MavlinkFTP::_valid_session(unsigned index)
@@ -645,3 +799,55 @@ MavlinkFTP::_return_request(Request *req)
 	_unlock_request_queue();
 }
 
+/// @brief Copy file (with limited space)
+int
+MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
+{
+	char buff[512];
+	int src_fd = -1, dst_fd = -1;
+	int op_errno = 0;
+
+	src_fd = ::open(src_path, O_RDONLY);
+	if (src_fd < 0) {
+		return -1;
+	}
+
+	dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY);
+	if (dst_fd < 0) {
+		op_errno = errno;
+		::close(src_fd);
+		errno = op_errno;
+		return -1;
+	}
+
+	while (length > 0) {
+		ssize_t bytes_read, bytes_written;
+		size_t blen = (length > sizeof(buff))? sizeof(buff) : length;
+
+		bytes_read = ::read(src_fd, buff, blen);
+		if (bytes_read == 0) {
+			// EOF
+			break;
+		}
+		else if (bytes_read < 0) {
+			warnx("cp: read");
+			op_errno = errno;
+			break;
+		}
+
+		bytes_written = ::write(dst_fd, buff, bytes_read);
+		if (bytes_written != bytes_read) {
+			warnx("cp: short write");
+			op_errno = errno;
+			break;
+		}
+
+		length -= bytes_written;
+	}
+
+	::close(src_fd);
+	::close(dst_fd);
+
+	errno = op_errno;
+	return (length > 0)? -1 : 0;
+}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 657e2f855..bef6775a9 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -89,13 +89,17 @@ public:
 		kCmdTerminateSession,	///< Terminates open Read session
 		kCmdResetSessions,	///< Terminates all open Read sessions
 		kCmdListDirectory,	///< List files in <path> from <offset>
-		kCmdOpenFile,		///< Opens file at <path> for reading, returns <session>
+		kCmdOpenFileRO,		///< Opens file at <path> for reading, returns <session>
 		kCmdReadFile,		///< Reads <size> bytes from <offset> in <session>
 		kCmdCreateFile,		///< Creates file at <path> for writing, returns <session>
-		kCmdWriteFile,		///< Appends <size> bytes to file in <session>
+		kCmdWriteFile,		///< Writes <size> bytes to <offset> in <session>
 		kCmdRemoveFile,		///< Remove file at <path>
 		kCmdCreateDirectory,	///< Creates directory at <path>
 		kCmdRemoveDirectory,	///< Removes Directory at <path>, must be empty
+		kCmdOpenFileWO,		///< Opens file at <path> for writing, returns <session>
+		kCmdTruncateFile,	///< Truncate file at <path> to <offset> length
+		kCmdRename,		///< Rename <path1> to <path2>
+		kCmdCalcFileCRC32,	///< Calculate CRC32 for file at <path>
 		
 		kRspAck = 128,		///< Ack response
 		kRspNak			///< Nak response
@@ -138,9 +142,10 @@ private:
 	static void	_worker_trampoline(void *arg);
 	void		_process_request(Request *req);
 	void		_reply(Request *req);
+	int		_copy_file(const char *src_path, const char *dst_path, ssize_t length);
 
 	ErrorCode	_workList(PayloadHeader *payload);
-	ErrorCode	_workOpen(PayloadHeader *payload, bool create);
+	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
 	ErrorCode	_workRead(PayloadHeader *payload);
 	ErrorCode	_workWrite(PayloadHeader *payload);
 	ErrorCode	_workTerminate(PayloadHeader *payload);
@@ -148,6 +153,9 @@ private:
 	ErrorCode	_workRemoveDirectory(PayloadHeader *payload);
 	ErrorCode	_workCreateDirectory(PayloadHeader *payload);
 	ErrorCode	_workRemoveFile(PayloadHeader *payload);
+	ErrorCode	_workTruncateFile(PayloadHeader *payload);
+	ErrorCode	_workRename(PayloadHeader *payload);
+	ErrorCode	_workCalcFileCRC32(PayloadHeader *payload);
 
 	static const unsigned	kRequestQueueSize = 2;			///< Max number of queued requests
 	Request			_request_bufs[kRequestQueueSize];	///< Request buffers which hold work
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
index 022041c74..4caa820b6 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;
 	
@@ -265,7 +265,7 @@ bool MavlinkFtpTest::_open_badfile_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*dir = "/foo";	// non-existent file
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -295,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void)
 		struct stat st;
 		const ReadTestCase *test = &_rgReadTestCases[i];
 		
-		payload.opcode = MavlinkFTP::kCmdOpenFile;
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
 		
 		bool success = _send_receive_msg(&payload,		// FTP payload header
@@ -342,7 +342,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*file = _rgReadTestCases[0].file;
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -400,7 +400,7 @@ bool MavlinkFtpTest::_read_test(void)
 		// Test case data files are created for specific boundary conditions
 		ut_compare("Test case data files are out of date", test->length, st.st_size);
 		
-		payload.opcode = MavlinkFTP::kCmdOpenFile;
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
 		
 		bool success = _send_receive_msg(&payload,		// FTP payload header
@@ -463,7 +463,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*file = _rgReadTestCases[0].file;
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -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 babd909da..2696192cc 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 d9c1e413a..10cbcb0ec 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 07cfce1dc..1cc28cce1 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/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index e9e03a44e..7fac69a61 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -37,6 +37,7 @@
  *
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <sys/types.h>
@@ -113,6 +114,7 @@ Mission::on_inactive()
 		update_offboard_mission();
 	}
 
+	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
 		_need_takeoff = true;
 	}
@@ -147,8 +149,19 @@ Mission::on_active()
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
-		advance_mission();
-		set_mission_items();
+		if (_mission_item.autocontinue) {
+			/* switch to next waypoint if 'autocontinue' flag set */
+			advance_mission();
+			set_mission_items();
+
+		} else {
+			/* else just report that item reached */
+			if (_mission_type == MISSION_TYPE_OFFBOARD) {
+				if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
+					set_mission_item_reached();
+				}
+			}
+		}
 
 	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
 		altitude_sp_foh_update();
@@ -221,7 +234,7 @@ Mission::update_offboard_mission()
 		_current_offboard_mission_index = 0;
 	}
 
-	report_current_offboard_mission_item();
+	set_current_offboard_mission_item();
 }
 
 
@@ -359,6 +372,10 @@ Mission::set_mission_items()
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+
+			/* use last setpoint for loiter */
+			_navigator->set_can_loiter_at_sp(true);
+
 		} else if (!user_feedback_done) {
 			/* only tell users that we got no mission if there has not been any
 			 * better, more specific feedback yet
@@ -375,10 +392,11 @@ Mission::set_mission_items()
 		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 		pos_sp_triplet->next.valid = false;
 
+		/* reuse setpoint for LOITER only if it's not IDLE */
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
 
 		reset_mission_item_reached();
-		report_mission_finished();
+		set_mission_finished();
 
 		_navigator->set_position_setpoint_triplet_updated();
 		return;
@@ -415,7 +433,7 @@ Mission::set_mission_items()
 			takeoff_alt += _navigator->get_home_position()->alt;
 		}
 
-		/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
+		/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
 		if (_navigator->get_vstatus()->condition_landed) {
 			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
 
@@ -423,32 +441,46 @@ Mission::set_mission_items()
 			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
 		}
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+		/* check if we already above takeoff altitude */
+		if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) {
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
-		_mission_item.lat = _navigator->get_global_position()->lat;
-		_mission_item.lon = _navigator->get_global_position()->lon;
-		_mission_item.altitude = takeoff_alt;
-		_mission_item.altitude_is_relative = false;
+			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+			_mission_item.lat = _navigator->get_global_position()->lat;
+			_mission_item.lon = _navigator->get_global_position()->lon;
+			_mission_item.altitude = takeoff_alt;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0;
 
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+			mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 
-	} else {
-		/* set current position setpoint from mission item */
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+			_navigator->set_position_setpoint_triplet_updated();
+			return;
 
-		/* require takeoff after landing or idle */
-		if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
-			_need_takeoff = true;
+		} else {
+			/* skip takeoff */
+			_takeoff = false;
 		}
+	}
 
-		_navigator->set_can_loiter_at_sp(false);
-		reset_mission_item_reached();
+	/* set current position setpoint from mission item */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 
-		if (_mission_type == MISSION_TYPE_OFFBOARD) {
-			report_current_offboard_mission_item();
-		}
-		// TODO: report onboard mission item somehow
+	/* require takeoff after landing or idle */
+	if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
+		_need_takeoff = true;
+	}
+
+	_navigator->set_can_loiter_at_sp(false);
+	reset_mission_item_reached();
+
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
+		set_current_offboard_mission_item();
+	}
+	// TODO: report onboard mission item somehow
 
+	if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
 		/* try to read next mission item */
 		struct mission_item_s mission_item_next;
 
@@ -460,6 +492,10 @@ Mission::set_mission_items()
 			/* next mission item is not available */
 			pos_sp_triplet->next.valid = false;
 		}
+
+	} else {
+		/* vehicle will be paused on current waypoint, don't set next item */
+		pos_sp_triplet->next.valid = false;
 	}
 
 	/* Save the distance between the current sp and the previous one */
@@ -666,19 +702,19 @@ Mission::save_offboard_mission_state()
 }
 
 void
-Mission::report_mission_item_reached()
+Mission::set_mission_item_reached()
 {
-	if (_mission_type == MISSION_TYPE_OFFBOARD) {
-		_navigator->get_mission_result()->reached = true;
-		_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
-	}
+	_navigator->get_mission_result()->reached = true;
+	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
 	_navigator->publish_mission_result();
 }
 
 void
-Mission::report_current_offboard_mission_item()
+Mission::set_current_offboard_mission_item()
 {
 	warnx("current offboard mission index: %d", _current_offboard_mission_index);
+	_navigator->get_mission_result()->reached = false;
+	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
 	_navigator->publish_mission_result();
 
@@ -686,9 +722,8 @@ Mission::report_current_offboard_mission_item()
 }
 
 void
-Mission::report_mission_finished()
+Mission::set_mission_finished()
 {
 	_navigator->get_mission_result()->finished = true;
 	_navigator->publish_mission_result();
 }
-
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 176529209..ea7cc0927 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -37,6 +37,7 @@
  *
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -130,19 +131,19 @@ private:
 	void save_offboard_mission_state();
 
 	/**
-	 * Report that a mission item has been reached
+	 * Set a mission item as reached
 	 */
-	void report_mission_item_reached();
+	void set_mission_item_reached();
 
 	/**
-	 * Rport the current mission item
+	 * Set the current offboard mission item
 	 */
-	void report_current_offboard_mission_item();
+	void set_current_offboard_mission_item();
 
 	/**
-	 * Report that the mission is finished if one exists or that none exists
+	 * Set that the mission is finished if one exists or that none exists
 	 */
-	void report_mission_finished();
+	void set_mission_finished();
 
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
@@ -154,8 +155,8 @@ private:
 
 	int _current_onboard_mission_index;
 	int _current_offboard_mission_index;
-	bool _need_takeoff;
-	bool _takeoff;
+	bool _need_takeoff;					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
+	bool _takeoff;						/**< takeoff state flag */
 
 	enum {
 		MISSION_TYPE_NONE,
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index c075903b7..fb8d07901 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -63,3 +63,5 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
index be3b9461a..b7568ce3a 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 1a5489709..8ed4efadf 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_ */