From 12390d7281985b7e3b6649fc9889e2e60a48dad1 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 4 May 2014 11:19:26 -0700
Subject: [PATCH 01/61] WIP: Mavlink file server

---
 src/modules/mavlink/mavlink_ftp.cpp      | 377 +++++++++++++++++++++++
 src/modules/mavlink/mavlink_ftp.h        | 211 +++++++++++++
 src/modules/mavlink/mavlink_receiver.cpp |   7 +
 src/modules/mavlink/mavlink_receiver.h   |   2 +
 src/modules/mavlink/module.mk            |   3 +-
 5 files changed, 599 insertions(+), 1 deletion(-)
 create mode 100644 src/modules/mavlink/mavlink_ftp.cpp
 create mode 100644 src/modules/mavlink/mavlink_ftp.h

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
new file mode 100644
index 0000000000..4cb31640ee
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -0,0 +1,377 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <crc32.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include "mavlink_ftp.h"
+
+MavlinkFTP *MavlinkFTP::_server;
+
+MavlinkFTP *
+MavlinkFTP::getServer()
+{
+	// XXX this really cries out for some locking...
+	if (_server == nullptr) {
+		_server = new MavlinkFTP;
+	}
+	return _server;
+}
+
+MavlinkFTP::MavlinkFTP()
+{
+	// initialise the request freelist
+	dq_init(&_workFree);
+	sem_init(&_lock, 0, 1);
+
+	// drop work entries onto the free list
+	for (unsigned i = 0; i < kRequestQueueSize; i++) {
+		_qFree(&_workBufs[i]);
+	}
+
+}
+
+void
+MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
+{
+	// get a free request
+	auto req = _dqFree();
+
+	// if we couldn't get a request slot, just drop it
+	if (req != nullptr) {
+
+		// decode the request
+		req->decode(channel, msg);
+
+		// and queue it for the worker
+		work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+	}
+}
+
+void
+MavlinkFTP::_workerTrampoline(void *arg)
+{
+	auto req = reinterpret_cast<Request *>(arg);
+	auto server = MavlinkFTP::getServer();
+
+	// call the server worker with the work item
+	server->_worker(req);
+}
+
+void
+MavlinkFTP::_worker(Request *req)
+{
+	auto hdr = req->header();
+	ErrorCode errorCode = kErrNone;
+	uint32_t messageCRC;
+
+	// basic sanity checks; must validate length before use
+	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+		errorCode = kErrNoRequest;
+		goto out;
+	}
+
+	// check request CRC to make sure this is one of ours
+	messageCRC = hdr->crc32;
+	hdr->crc32 = 0;
+	if (crc32(req->data(), req->dataSize()) != messageCRC) {
+		errorCode = kErrNoRequest;
+		goto out;
+	}
+
+	switch (hdr->opcode) {
+	case kCmdNone:
+		break;
+
+	case kCmdTerminate:
+		if (!Session::terminate(hdr->session)) {
+			errorCode = kErrNoRequest;
+		}
+		break;
+
+	case kCmdReset:
+		Session::reset();
+		break;
+
+	case kCmdList:
+		errorCode = _workList(req);
+		break;
+
+	case kCmdOpen:
+		errorCode = _workOpen(req, false);
+		break;
+
+	case kCmdCreate:
+		errorCode = _workOpen(req, true);
+		break;
+
+	case kCmdRead:
+		errorCode = _workRead(req);
+		break;
+
+	case kCmdWrite:
+		errorCode = _workWrite(req);
+		break;
+
+	case kCmdRemove:
+		errorCode = _workRemove(req);
+		break;
+
+	default:
+		errorCode = kErrNoRequest;
+		break;	
+	}
+
+out:
+	// handle success vs. error
+	if (errorCode == kErrNone) {
+		hdr->opcode = kRspAck;
+	} else {
+		hdr->opcode = kRspNak;
+		hdr->size = 1;
+		hdr->data[0] = errorCode;
+	}
+
+	// respond to the request
+	_reply(req);
+
+	// free the request buffer back to the freelist
+	_qFree(req);
+}
+
+void
+MavlinkFTP::_reply(Request *req)
+{
+	auto hdr = req->header();
+
+	// message is assumed to be already constructed in the request buffer, so generate the CRC
+	hdr->crc32 = 0;
+	hdr->crc32 = crc32(req->data(), req->dataSize());
+
+	// then pack and send the reply back to the request source
+	mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->data());
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workList(Request *req)
+{
+	auto hdr = req->header();
+
+	// open directory
+
+	// seek in directory
+
+	// read entries until buffer is full
+
+	// send reply
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workOpen(Request *req, bool create)
+{
+	auto hdr = req->header();
+
+	// allocate session ID
+	int session = Session::allocate();
+	if (session < 0) {
+		return kErrNoSession;
+	}
+
+	// get the session to open the file
+	if (!Session::get(session)->open(req->dataAsCString(), create)) {
+		return create ? kErrPerm : kErrNotFile;
+	}
+
+	// save the session ID in the reply
+	hdr->session = session;
+	hdr->size = 0;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRead(Request *req)
+{
+	auto hdr = req->header();
+
+	// look up session
+	auto session = Session::get(hdr->session);
+	if (session == nullptr) {
+		return kErrNoSession;
+	}
+
+	// read from file
+	int result = session->read(hdr->offset, &hdr->data[0], hdr->size);
+
+	if (result < 0) {
+		return kErrIO;
+	}
+	hdr->size = result;
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workWrite(Request *req)
+{
+	auto hdr = req->header();
+
+	// look up session
+	auto session = Session::get(hdr->session);
+	if (session == nullptr) {
+		return kErrNoSession;
+	}
+
+	// append to file
+	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+
+	if (result < 0) {
+		// XXX might also be no space, I/O, etc.
+		return kErrNotAppend;
+	}
+
+	hdr->size = result;
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemove(Request *req)
+{
+	auto hdr = req->header();
+
+	// for now, send error reply
+	return kErrPerm;
+}
+
+MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession];
+
+int
+MavlinkFTP::Session::allocate()
+{
+	for (unsigned i = 0; i < kMaxSession; i++) {
+		if (_sessions[i]._fd < 0) {
+			return i;
+		}
+	}
+	return -1;
+}
+
+MavlinkFTP::Session *
+MavlinkFTP::Session::get(unsigned index)
+{
+	if ((index >= kMaxSession) || (_sessions[index]._fd < 0)) {
+		return nullptr;
+	}
+	return &_sessions[index];
+}
+
+void
+MavlinkFTP::Session::terminate()
+{
+	// clean up aborted transfers?
+	if (_fd >= 0) {
+		close(_fd);
+		_fd = -1;
+	}
+}
+
+bool
+MavlinkFTP::Session::terminate(unsigned index)
+ {
+	Session *session = get(index);
+
+	if (session == nullptr) {
+		return false;
+	}
+
+	session->terminate();
+	return true;
+}
+
+void
+MavlinkFTP::Session::reset()
+{
+	for (unsigned i = 0; i < kMaxSession; i++) {
+		terminate(i);
+	}
+}
+
+bool
+MavlinkFTP::Session::open(const char *path, bool create)
+{
+	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
+
+	_fd = open(path, oflag);
+	if (_fd < 0) {
+		return false;
+	}
+	return true;
+}
+
+int
+MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count)
+{
+	// can we seek to the location?
+	if (lseek(_fd, offset, SEEK_SET) < 0) {
+		return -1;
+	}
+
+	return read(_fd, buf, count);
+}
+
+int
+MavlinkFTP::Session::append(off_t offset, uint8_t *buf, uint8_t count)
+{
+	// make sure that the requested offset matches our current position
+	off_t pos = lseek(_fd, 0, SEEK_CUR);
+	if (pos != offset) {
+		return -1;
+	}
+	return write(_fd, buf, count);
+}
+
+char *
+MavlinkFTP::Request::dataAsCString()
+{
+	// guarantee nul termination
+	if (header()->size < kMaxDataLength) {
+		data()[header()->size] = '\0';
+	} else {
+		data()[kMaxDataLength - 1] = '\0';
+	}
+
+	// and return data
+	return (char *)data();
+}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
new file mode 100644
index 0000000000..a4f67793e3
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -0,0 +1,211 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @file mavlink_ftp.h
+ *
+ * MAVLink remote file server.
+ *
+ * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
+ * a session ID and sequence number.
+ *
+ * A limited number of requests (currently 2) may be outstanding at a time.
+ * Additional messages will be discarded.
+ *
+ * Messages consist of a fixed header, followed by a data area.
+ *
+ */
+ 
+#include <dirent.h>
+#include <queue.h>
+
+#include <nuttx/wqueue.h>
+
+#include "mavlink_messages.h"
+
+class MavlinkFTP
+{
+public:
+	MavlinkFTP();
+
+	static MavlinkFTP	*getServer();
+
+	// static interface
+	void			handle_message(mavlink_message_t *msg,
+					       mavlink_channel_t channel);
+
+private:
+
+	static const unsigned	kRequestQueueSize = 2;
+
+	static MavlinkFTP	*_server;
+
+	struct RequestHeader
+	{
+		uint8_t		magic;
+		uint8_t		session;
+		uint8_t		opcode;
+		uint8_t		size;
+		uint32_t	crc32;
+		uint32_t	offset;
+		uint8_t		data[];
+	};
+
+	struct FileList
+	{
+		uint32_t	fileSize;
+		uint8_t		nameLength;
+		uint8_t		name[];
+	};
+
+	enum Opcode : uint8_t
+	{
+		kCmdNone,	// ignored, always acked
+		kCmdTerminate,	// releases sessionID, closes file
+		kCmdReset,	// terminates all sessions
+		kCmdList,	// list files in <path> from <offset>
+		kCmdOpen,	// opens <path> for reading, returns <session>
+		kCmdRead,	// reads <size> bytes from <offset> in <session>
+		kCmdCreate,	// creates <path> for writing, returns <session>
+		kCmdWrite,	// appends <size> bytes at <offset> in <session>
+		kCmdRemove,	// remove file (only if created by server?)
+
+		kRspAck,
+		kRspNak
+	};
+
+	enum ErrorCode : uint8_t
+	{
+		kErrNone,
+		kErrNoRequest,
+		kErrNoSession,
+		kErrSequence,
+		kErrNotDir,
+		kErrNotFile,
+		kErrEOF,
+		kErrNotAppend,
+		kErrTooBig,
+		kErrIO,
+		kErrPerm
+	};
+
+	class Session
+	{
+	public:
+		Session() : _fd(-1) {}
+
+		static int	allocate();
+		static Session	*get(unsigned index);
+		static bool	terminate(unsigned index);
+		static void	reset();
+
+		void		terminate();
+		bool		open(const char *path, bool create);
+		int		read(off_t offset, uint8_t *buf, uint8_t count);
+		int		append(off_t offset, uint8_t *buf, uint8_t count);
+
+	private:
+		static const unsigned kMaxSession = 2;
+		static Session	_sessions[kMaxSession];
+
+		int		_fd;
+	};
+
+	class Request
+	{
+	public:
+		union {
+			dq_entry_t	entry;
+			work_s		work;
+		};
+		mavlink_channel_t	channel;
+
+		void		decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) {
+			channel = fromChannel;
+			mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+		}
+
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		uint8_t		*data()    { return &_message.data[0]; }
+		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
+		uint16_t	sequence() const { return _message.seqnr; }
+
+		char		*dataAsCString();
+
+	private:
+		mavlink_encapsulated_data_t _message;
+
+	};
+
+	static const uint8_t	kProtocolMagic = 'f';
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+
+	/// Request worker; runs on the low-priority work queue to service
+	/// remote requests.
+	///
+	static void		_workerTrampoline(void *arg);
+	void			_worker(Request *req);
+
+	/// Reply to a request (XXX should be a Request method)
+	///
+	void			_reply(Request *req);
+
+	ErrorCode		_workList(Request *req);
+	ErrorCode		_workOpen(Request *req, bool create);
+	ErrorCode		_workRead(Request *req);
+	ErrorCode		_workWrite(Request *req);
+	ErrorCode		_workRemove(Request *req);
+
+	// work freelist
+	Request			_workBufs[kRequestQueueSize];
+	dq_queue_t		_workFree;
+	sem_t			_lock;
+
+	void			_qLock() { do {} while (sem_wait(&_lock) != 0); }
+	void			_qUnlock() { sem_post(&_lock); }
+
+	void			_qFree(Request *req) {
+		_qLock();
+		dq_addlast(&req->entry, &_workFree);
+		_qUnlock();
+	}
+
+	Request			*_dqFree() {
+		_qLock();
+		auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
+		_qUnlock();
+		return req;
+	}
+};
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7c93c1c009..fd1abe5eee 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -112,6 +112,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_hil_local_alt0(0.0)
 {
 	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
+
+	// make sure the FTP server is started
+	(void)MavlinkFTP::getServer();
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -150,6 +153,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_manual_control(msg);
 		break;
 
+	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+		MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel());
+		break;
+
 	default:
 		break;
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 9ab84b58a3..36e6143ac9 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -68,6 +68,8 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 
+#include "mavlink_ftp.h"
+
 class Mavlink;
 
 class MavlinkReceiver
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index dcca11977f..c348a33dbf 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -43,7 +43,8 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
 			mavlink_rate_limiter.cpp \
-			mavlink_commands.cpp
+			mavlink_commands.cpp \
+			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 

From ef7c57f1cece0e49cd95e7bbdc0d6563eca6a9eb Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 4 May 2014 12:25:25 -0700
Subject: [PATCH 02/61] Implement directory listing

---
 src/modules/mavlink/mavlink_ftp.cpp | 51 ++++++++++++++++++++++++++---
 src/modules/mavlink/mavlink_ftp.h   | 10 ++----
 2 files changed, 49 insertions(+), 12 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 4cb31640ee..16f96f2ccb 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -187,16 +187,57 @@ MavlinkFTP::ErrorCode
 MavlinkFTP::_workList(Request *req)
 {
 	auto hdr = req->header();
+	DIR *dp = opendir(req->dataAsCString());
 
-	// open directory
+	if (dp == nullptr) {
+		return kErrNotDir;
+	}
 
-	// seek in directory
+	ErrorCode errorCode = kErrNone;
+	struct dirent entry, *result = nullptr;
+	unsigned offset = 0;
 
-	// read entries until buffer is full
+	// move to the requested offset
+	seekdir(dp, hdr->offset);
 
-	// send reply
+	for (;;) {
+		// read the directory entry
+		if (readdir_r(dp, &entry, &result)) {
+			errorCode = kErrIO;
+			break;
+		}
 
-	return kErrNone;
+		// no more entries?
+		if (result == nullptr) {
+			break;
+		}
+
+		// name too big to fit?
+		if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
+			break;
+		}
+
+		// store the type marker
+		switch (entry.d_type) {
+		case DTYPE_FILE:
+			hdr->data[offset++] = kDirentFile;
+			break;
+		case DTYPE_DIRECTORY:
+			hdr->data[offset++] = kDirentDir;
+			break;
+		default:
+			hdr->data[offset++] = kDirentUnknown;
+			break;
+		}
+
+		// copy the name, which we know will fit
+		strcpy((char *)&hdr->data[offset], entry.d_name);
+	}
+
+	closedir(dp);
+	hdr->size = offset;
+
+	return errorCode;
 }
 
 MavlinkFTP::ErrorCode
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index a4f67793e3..9615f7200f 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -83,13 +83,6 @@ private:
 		uint8_t		data[];
 	};
 
-	struct FileList
-	{
-		uint32_t	fileSize;
-		uint8_t		nameLength;
-		uint8_t		name[];
-	};
-
 	enum Opcode : uint8_t
 	{
 		kCmdNone,	// ignored, always acked
@@ -170,6 +163,9 @@ private:
 	};
 
 	static const uint8_t	kProtocolMagic = 'f';
+	static const char	kDirentFile = 'F';
+	static const char	kDirentDir = 'D';
+	static const char	kDirentUnknown = 'U';
 	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
 
 	/// Request worker; runs on the low-priority work queue to service

From 6351fd1e2cf9e2f7448558b3516ce84a988ff3da Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 5 May 2014 13:48:05 +0200
Subject: [PATCH 03/61] Added debug printfs

---
 src/modules/mavlink/mavlink_ftp.h | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 9615f7200f..eab2a567ab 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -52,6 +52,7 @@
 #include <queue.h>
 
 #include <nuttx/wqueue.h>
+#include <systemlib/err.h>
 
 #include "mavlink_messages.h"
 
@@ -146,8 +147,16 @@ private:
 		mavlink_channel_t	channel;
 
 		void		decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) {
-			channel = fromChannel;
-			mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+			switch (fromMessage->msgid) {
+
+				case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+					channel = fromChannel;
+					mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+					warnx("got enc data");
+					break;
+				default:
+					warnx("unknown msg");
+			}
 		}
 
 		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }

From bd9d58f565383598db785908a7cc08f87f6a07f1 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 12 May 2014 23:06:45 +0200
Subject: [PATCH 04/61] attitude_estimator_ekf: auto detect mag declination
 using GPS coordinates

---
 src/lib/geo/geo.h                             |  2 +-
 .../attitude_estimator_ekf_main.cpp           | 26 +++++++++++++++----
 2 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index e2f3da6f80..87c1cf4605 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -50,7 +50,7 @@
 
 __BEGIN_DECLS
 
-#include "geo/geo_mag_declination.h"
+#include "geo_mag_declination.h"
 
 #include <stdbool.h>
 
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index c61b6ff3fe..e10c7de560 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -64,6 +64,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
@@ -292,6 +293,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
 	unsigned offset_count = 0;
 
+	/* magnetic declination, in radians */
+	float mag_decl = 0.0f;
+
 	/* rotation matrix for magnetic declination */
 	math::Matrix<3, 3> R_decl;
 	R_decl.identity();
@@ -330,9 +334,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 				/* update parameters */
 				parameters_update(&ekf_param_handles, &ekf_params);
-
-				/* update mag declination rotation matrix */
-				R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
 			}
 
 			/* only run filter if sensor values changed */
@@ -345,6 +346,13 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				orb_check(sub_gps, &gps_updated);
 				if (gps_updated) {
 					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
+
+					if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
+
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, mag_decl);
+					}
 				}
 
 				bool global_pos_updated;
@@ -474,7 +482,15 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						parameters_update(&ekf_param_handles, &ekf_params);
 
 						/* update mag declination rotation matrix */
-						R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
+						if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
+
+						} else {
+							mag_decl = ekf_params.mag_decl;
+						}
+
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, mag_decl);
 
 						x_aposteriori_k[0] = z_k[0];
 						x_aposteriori_k[1] = z_k[1];
@@ -522,7 +538,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					att.roll = euler[0];
 					att.pitch = euler[1];
-					att.yaw = euler[2] + ekf_params.mag_decl;
+					att.yaw = euler[2] + mag_decl;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];

From 99dfef357b2f9228df54f0f26481c37204afa591 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sat, 7 Jun 2014 12:54:04 -0700
Subject: [PATCH 05/61] fix extraction of path info from FTP request

---
 src/modules/mavlink/mavlink_ftp.cpp | 20 ++++++++++++++------
 src/modules/mavlink/mavlink_ftp.h   |  3 ++-
 2 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 16f96f2ccb..7a72835cc0 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -33,6 +33,7 @@
 
 #include <crc32.h>
 #include <unistd.h>
+#include <stdio.h>
 #include <fcntl.h>
 
 #include "mavlink_ftp.h"
@@ -105,11 +106,14 @@ MavlinkFTP::_worker(Request *req)
 	// check request CRC to make sure this is one of ours
 	messageCRC = hdr->crc32;
 	hdr->crc32 = 0;
-	if (crc32(req->data(), req->dataSize()) != messageCRC) {
+	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
 		errorCode = kErrNoRequest;
 		goto out;
+		printf("ftp: bad crc\n");
 	}
 
+	printf("ftp: opc %u size %u offset %u\n", hdr->opcode, hdr->size, hdr->offset);
+
 	switch (hdr->opcode) {
 	case kCmdNone:
 		break;
@@ -157,7 +161,9 @@ out:
 	// handle success vs. error
 	if (errorCode == kErrNone) {
 		hdr->opcode = kRspAck;
+		printf("FTP: ack\n");
 	} else {
+		printf("FTP: nak %u\n", errorCode);
 		hdr->opcode = kRspNak;
 		hdr->size = 1;
 		hdr->data[0] = errorCode;
@@ -177,10 +183,10 @@ MavlinkFTP::_reply(Request *req)
 
 	// message is assumed to be already constructed in the request buffer, so generate the CRC
 	hdr->crc32 = 0;
-	hdr->crc32 = crc32(req->data(), req->dataSize());
+	hdr->crc32 = crc32(req->rawData(), req->dataSize());
 
 	// then pack and send the reply back to the request source
-	mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->data());
+	mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->rawData());
 }
 
 MavlinkFTP::ErrorCode
@@ -190,6 +196,7 @@ MavlinkFTP::_workList(Request *req)
 	DIR *dp = opendir(req->dataAsCString());
 
 	if (dp == nullptr) {
+		printf("FTP: can't open path '%s'\n", req->dataAsCString());
 		return kErrNotDir;
 	}
 
@@ -232,6 +239,7 @@ MavlinkFTP::_workList(Request *req)
 
 		// copy the name, which we know will fit
 		strcpy((char *)&hdr->data[offset], entry.d_name);
+		printf("FTP: list %s\n", entry.d_name);
 	}
 
 	closedir(dp);
@@ -408,11 +416,11 @@ MavlinkFTP::Request::dataAsCString()
 {
 	// guarantee nul termination
 	if (header()->size < kMaxDataLength) {
-		data()[header()->size] = '\0';
+		requestData()[header()->size] = '\0';
 	} else {
-		data()[kMaxDataLength - 1] = '\0';
+		requestData()[kMaxDataLength - 1] = '\0';
 	}
 
 	// and return data
-	return (char *)data();
+	return (char *)&(header()->data[0]);
 }
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index eab2a567ab..6a2414613f 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -159,8 +159,9 @@ private:
 			}
 		}
 
+		uint8_t		*rawData() { return &_message.data[0]; }
 		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
-		uint8_t		*data()    { return &_message.data[0]; }
+		uint8_t         *requestData() { return &(header()->data[0]); }
 		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
 		uint16_t	sequence() const { return _message.seqnr; }
 

From e8906619005a09d98fe68b04f2867c537f0895a8 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sat, 7 Jun 2014 14:46:46 -0700
Subject: [PATCH 06/61] Don't queue empty work items.

---
 src/modules/mavlink/mavlink_ftp.cpp | 13 ++++++++-----
 src/modules/mavlink/mavlink_ftp.h   | 23 ++++++++++++-----------
 2 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 7a72835cc0..d4d659d91b 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -73,10 +73,13 @@ MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
 	if (req != nullptr) {
 
 		// decode the request
-		req->decode(channel, msg);
+		if (req->decode(msg, channel)) {
 
-		// and queue it for the worker
-		work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+			// and queue it for the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+		} else {
+			_qFree(req);
+		}
 	}
 }
 
@@ -112,7 +115,7 @@ MavlinkFTP::_worker(Request *req)
 		printf("ftp: bad crc\n");
 	}
 
-	printf("ftp: opc %u size %u offset %u\n", hdr->opcode, hdr->size, hdr->offset);
+	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
 
 	switch (hdr->opcode) {
 	case kCmdNone:
@@ -186,7 +189,7 @@ MavlinkFTP::_reply(Request *req)
 	hdr->crc32 = crc32(req->rawData(), req->dataSize());
 
 	// then pack and send the reply back to the request source
-	mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->rawData());
+	req->reply();
 }
 
 MavlinkFTP::ErrorCode
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 6a2414613f..f68dab98d5 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -144,19 +144,18 @@ private:
 			dq_entry_t	entry;
 			work_s		work;
 		};
-		mavlink_channel_t	channel;
 
-		void		decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) {
-			switch (fromMessage->msgid) {
-
-				case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
-					channel = fromChannel;
-					mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
-					warnx("got enc data");
-					break;
-				default:
-					warnx("unknown msg");
+		bool		decode(mavlink_message_t *fromMessage, mavlink_channel_t fromChannel) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+				_channel = fromChannel;
+				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+				return true;
 			}
+			return false;
+		}
+
+		void		reply() {
+			mavlink_msg_encapsulated_data_send(_channel, sequence(), rawData());
 		}
 
 		uint8_t		*rawData() { return &_message.data[0]; }
@@ -164,10 +163,12 @@ private:
 		uint8_t         *requestData() { return &(header()->data[0]); }
 		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
 		uint16_t	sequence() const { return _message.seqnr; }
+		mavlink_channel_t &channel() { return _channel; }
 
 		char		*dataAsCString();
 
 	private:
+		mavlink_channel_t	_channel;
 		mavlink_encapsulated_data_t _message;
 
 	};

From 2235a86a66c1c9dd70c43323f8b801d7e03e7436 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 8 Jun 2014 18:47:38 +0200
Subject: [PATCH 07/61] Support link forwarding

---
 ROMFS/px4fmu_common/init.d/rc.usb | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 76593881db..02002b55ff 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -3,7 +3,7 @@
 # USB MAVLink start
 #
 
-mavlink start -r 10000 -d /dev/ttyACM0
+mavlink start -r 10000 -d /dev/ttyACM0 -x
 # Enable a number of interesting streams we want via USB
 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
 usleep 100000

From 4b70a0d04687ee21c21113acea73fc96088a0d86 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 8 Jun 2014 18:48:31 +0200
Subject: [PATCH 08/61] Added include order warning

---
 mavlink/include/mavlink/v1.0/common/common.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h
index 020211d406..de6e22011d 100644
--- a/mavlink/include/mavlink/v1.0/common/common.h
+++ b/mavlink/include/mavlink/v1.0/common/common.h
@@ -5,6 +5,10 @@
 #ifndef COMMON_H
 #define COMMON_H
 
+#ifndef MAVLINK_H
+ #error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually.
+#endif
+
 #ifdef __cplusplus
 extern "C" {
 #endif

From dfe71b615c3d88d223a91559912b0c7d7f0cad0f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 8 Jun 2014 18:50:36 +0200
Subject: [PATCH 09/61] MAVLink helper function fixes

---
 mavlink/include/mavlink/v1.0/checksum.h       |  2 +-
 .../include/mavlink/v1.0/mavlink_helpers.h    | 21 ++++++++++++-------
 mavlink/include/mavlink/v1.0/mavlink_types.h  |  4 ++--
 3 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/mavlink/include/mavlink/v1.0/checksum.h b/mavlink/include/mavlink/v1.0/checksum.h
index 948e080a14..7d9b6ac0c7 100644
--- a/mavlink/include/mavlink/v1.0/checksum.h
+++ b/mavlink/include/mavlink/v1.0/checksum.h
@@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
  * @param data new bytes to hash
  * @param crcAccum the already accumulated checksum
  **/
-static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
+static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length)
 {
 	const uint8_t *p = (const uint8_t *)pBuffer;
 	while (length--) {
diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h
index 96672f847f..1639a830bb 100644
--- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h
+++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h
@@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
 #endif
 {
 	// This code part is the same for all messages;
-	uint16_t checksum;
 	msg->magic = MAVLINK_STX;
 	msg->len = length;
 	msg->sysid = system_id;
@@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
 	// One sequence number per component
 	msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
 	mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
-	checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
+	msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
+	crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
 #if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
+	crc_accumulate(crc_extra, &msg->checksum);
 #endif
-	mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
-	mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
+	mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
+	mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
 
 	return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 }
@@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint
 	buf[4] = mavlink_system.compid;
 	buf[5] = msgid;
 	status->current_tx_seq++;
-	checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
+	checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
 	crc_accumulate_buffer(&checksum, packet, length);
 #if MAVLINK_CRC_EXTRA
 	crc_accumulate(crc_extra, &checksum);
@@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
 
 	ck[0] = (uint8_t)(msg->checksum & 0xFF);
 	ck[1] = (uint8_t)(msg->checksum >> 8);
+	// XXX use the right sequence here
 
 	MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
 	_mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
@@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
  */
 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
 {
-	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
+	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
+
+	uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
+
+	ck[0] = (uint8_t)(msg->checksum & 0xFF);
+	ck[1] = (uint8_t)(msg->checksum >> 8);
+
 	return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
 }
 
diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h
index 4019c619e0..43658e629e 100644
--- a/mavlink/include/mavlink/v1.0/mavlink_types.h
+++ b/mavlink/include/mavlink/v1.0/mavlink_types.h
@@ -28,6 +28,7 @@
 
 #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
 
+#pragma pack(push, 1)
 typedef struct param_union {
 	union {
 		float param_float;
@@ -62,13 +63,12 @@ typedef struct __mavlink_message {
 	uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
 } mavlink_message_t;
 
-
 typedef struct __mavlink_extended_message {
        mavlink_message_t base_msg;
        int32_t extended_payload_len;   ///< Length of extended payload if any
        uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
 } mavlink_extended_message_t;
-
+#pragma pack(pop)
 
 typedef enum {
 	MAVLINK_TYPE_CHAR     = 0,

From f84e18f27a84254c9760a771f8ad91f864ba25fe Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 8 Jun 2014 18:51:17 +0200
Subject: [PATCH 10/61] Formatting and some ftp drive-by

---
 src/modules/mavlink/mavlink_main.h | 172 ++++++++++++++++-------------
 1 file changed, 94 insertions(+), 78 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 40edc4b851..f0ca028a83 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -123,27 +123,41 @@ public:
 	/**
 	 * Display the mavlink status.
 	 */
-	void		status();
+	void			status();
 
-	static int stream(int argc, char *argv[]);
+	static int		stream(int argc, char *argv[]);
 
-	static int	instance_count();
+	static int		instance_count();
 
-	static Mavlink *new_instance();
+	static Mavlink		*new_instance();
 
-	static Mavlink *get_instance(unsigned instance);
+	static Mavlink		*get_instance(unsigned instance);
 
-	static Mavlink *get_instance_for_device(const char *device_name);
+	static Mavlink		*get_instance_for_device(const char *device_name);
 
-	static int	destroy_all_instances();
+	static int		destroy_all_instances();
 
-	static bool	instance_exists(const char *device_name, Mavlink *self);
+	static bool		instance_exists(const char *device_name, Mavlink *self);
 
-	static void	forward_message(mavlink_message_t *msg, Mavlink *self);
+	static void		forward_message(mavlink_message_t *msg, Mavlink *self);
 
-	static int get_uart_fd(unsigned index);
+	static int		get_uart_fd(unsigned index);
 
-	int get_uart_fd();
+	int			get_uart_fd();
+
+	/**
+	 * Get the MAVLink system id.
+	 *
+	 * @return		The system ID of this vehicle
+	 */
+	int			get_system_id();
+
+	/**
+	 * Get the MAVLink component id.
+	 *
+	 * @return		The component ID of this vehicle
+	 */
+	int			get_component_id();
 
 	const char *_device_name;
 
@@ -153,30 +167,30 @@ public:
 		MAVLINK_MODE_CAMERA
 	};
 
-	void		set_mode(enum MAVLINK_MODE);
-	enum MAVLINK_MODE		get_mode() { return _mode; }
+	void			set_mode(enum MAVLINK_MODE);
+	enum MAVLINK_MODE	get_mode() { return _mode; }
 
-	bool		get_hil_enabled() { return _hil_enabled; }
+	bool			get_hil_enabled() { return _hil_enabled; }
 
-	bool		get_use_hil_gps() { return _use_hil_gps; }
+	bool			get_use_hil_gps() { return _use_hil_gps; }
 
-	bool		get_flow_control_enabled() { return _flow_control_enabled; }
+	bool			get_flow_control_enabled() { return _flow_control_enabled; }
 
-	bool		get_forwarding_on() { return _forwarding_on; }
+	bool			get_forwarding_on() { return _forwarding_on; }
 
 	/**
 	 * Handle waypoint related messages.
 	 */
-	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
+	void			mavlink_wpm_message_handler(const mavlink_message_t *msg);
 
-	static int start_helper(int argc, char *argv[]);
+	static int		start_helper(int argc, char *argv[]);
 
 	/**
 	 * Handle parameter related messages.
 	 */
-	void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+	void			mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
 
-	void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+	void			get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
 
 	/**
 	 * Enable / disable Hardware in the Loop simulation mode.
@@ -186,90 +200,93 @@ public:
 	 *			requested change could not be made or was
 	 *			redundant.
 	 */
-	int		set_hil_enabled(bool hil_enabled);
+	int			set_hil_enabled(bool hil_enabled);
 
-	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+	MavlinkOrbSubscription	*add_orb_subscription(const orb_id_t topic);
 
-	int get_instance_id();
+	int			get_instance_id();
 
 	/**
 	 * Enable / disable hardware flow control.
 	 *
 	 * @param enabled	True if hardware flow control should be enabled
 	 */
-	int enable_flow_control(bool enabled);
+	int			enable_flow_control(bool enabled);
 
-	mavlink_channel_t get_channel();
+	const mavlink_channel_t	get_channel();
 
-	bool		_task_should_exit;		/**< if true, mavlink task should exit */
+	bool			_task_should_exit;	/**< if true, mavlink task should exit */
 
-	int get_mavlink_fd() { return _mavlink_fd; }
+	int			get_mavlink_fd() { return _mavlink_fd; }
 
 
 	/* Functions for waiting to start transmission until message received. */
-	void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
-	bool get_has_received_messages() { return _received_messages; }
-	void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
-	bool get_wait_to_transmit() { return _wait_to_transmit; }
-	bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+	void			set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
+	bool			get_has_received_messages() { return _received_messages; }
+	void			set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
+	bool			get_wait_to_transmit() { return _wait_to_transmit; }
+	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+
+	bool			message_buffer_write(void *ptr, int size);
 
 protected:
-	Mavlink	*next;
+	Mavlink			*next;
 
 private:
-	int _instance_id;
+	int			_instance_id;
 
-	int		_mavlink_fd;
-	bool		_task_running;
+	int			_mavlink_fd;
+	bool			_task_running;
 
 	/* states */
-	bool		_hil_enabled;		/**< Hardware In the Loop mode */
-	bool		_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
-	bool		_is_usb_uart;		/**< Port is USB */
-	bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
-	bool        _received_messages; /**< Whether we've received valid mavlink messages. */
+	bool			_hil_enabled;		/**< Hardware In the Loop mode */
+	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
+	bool			_is_usb_uart;		/**< Port is USB */
+	bool        		_wait_to_transmit;  	/**< Wait to transmit until received messages. */
+	bool        		_received_messages;	/**< Whether we've received valid mavlink messages. */
 
-	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
+	unsigned		_main_loop_delay;	/**< mainloop delay, depends on data rate */
 
-	MavlinkOrbSubscription *_subscriptions;
-	MavlinkStream *_streams;
+	MavlinkOrbSubscription	*_subscriptions;
+	MavlinkStream		*_streams;
 
-	orb_advert_t	_mission_pub;
-	struct mission_s mission;
-	MAVLINK_MODE _mode;
+	orb_advert_t		_mission_pub;
+	struct mission_s	mission;
+	MAVLINK_MODE		_mode;
 
-	uint8_t _mavlink_wpm_comp_id;
-	mavlink_channel_t _channel;
+	uint8_t			_mavlink_wpm_comp_id;
+	mavlink_channel_t	_channel;
 
 	struct mavlink_logbuffer _logbuffer;
-	unsigned int _total_counter;
+	unsigned int		_total_counter;
 
-	pthread_t _receive_thread;
+	pthread_t		_receive_thread;
 
 	/* Allocate storage space for waypoints */
-	mavlink_wpm_storage _wpm_s;
-	mavlink_wpm_storage *_wpm;
+	mavlink_wpm_storage	_wpm_s;
+	mavlink_wpm_storage	*_wpm;
 
-	bool _verbose;
-	bool _forwarding_on;
-	bool _passing_on;
-	int _uart_fd;
-	int _baudrate;
-	int _datarate;
+	bool			_verbose;
+	bool			_forwarding_on;
+	bool			_passing_on;
+	bool			_ftp_on;
+	int			_uart_fd;
+	int			_baudrate;
+	int			_datarate;
 
 	/**
 	 * If the queue index is not at 0, the queue sending
 	 * logic will send parameters from the current index
 	 * to len - 1, the end of the param list.
 	 */
-	unsigned int _mavlink_param_queue_index;
+	unsigned int		_mavlink_param_queue_index;
 
-	bool mavlink_link_termination_allowed;
+	bool			mavlink_link_termination_allowed;
 
-	char 	*_subscribe_to_stream;
-	float	_subscribe_to_stream_rate;
+	char 			*_subscribe_to_stream;
+	float			_subscribe_to_stream_rate;
 
-	bool		_flow_control_enabled;
+	bool			_flow_control_enabled;
 
 	struct mavlink_message_buffer {
 		int write_ptr;
@@ -277,11 +294,12 @@ private:
 		int size;
 		char *data;
 	};
-	mavlink_message_buffer _message_buffer;
 
-	pthread_mutex_t _message_buffer_mutex;
+	mavlink_message_buffer	_message_buffer;
 
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	pthread_mutex_t		_message_buffer_mutex;
+
+	perf_counter_t		_loop_perf;			/**< loop performance counter */
 
 	/**
 	 * Send one parameter.
@@ -289,7 +307,7 @@ private:
 	 * @param param		The parameter id to send.
 	 * @return		zero on success, nonzero on failure.
 	 */
-	int mavlink_pm_send_param(param_t param);
+	int			mavlink_pm_send_param(param_t param);
 
 	/**
 	 * Send one parameter identified by index.
@@ -297,7 +315,7 @@ private:
 	 * @param index		The index of the parameter to send.
 	 * @return		zero on success, nonzero else.
 	 */
-	int mavlink_pm_send_param_for_index(uint16_t index);
+	int			mavlink_pm_send_param_for_index(uint16_t index);
 
 	/**
 	 * Send one parameter identified by name.
@@ -305,14 +323,14 @@ private:
 	 * @param name		The index of the parameter to send.
 	 * @return		zero on success, nonzero else.
 	 */
-	int mavlink_pm_send_param_for_name(const char *name);
+	int			mavlink_pm_send_param_for_name(const char *name);
 
 	/**
 	 * Send a queue of parameters, one parameter per function call.
 	 *
 	 * @return		zero on success, nonzero on failure
 	 */
-	int mavlink_pm_queued_send(void);
+	int			mavlink_pm_queued_send(void);
 
 	/**
 	 * Start sending the parameter queue.
@@ -322,12 +340,12 @@ private:
 	 * mavlink_pm_queued_send().
 	 * @see 		mavlink_pm_queued_send()
 	 */
-	void mavlink_pm_start_queued_send();
+	void			mavlink_pm_start_queued_send();
 
-	void mavlink_update_system();
+	void			mavlink_update_system();
 
-	void mavlink_waypoint_eventloop(uint64_t now);
-	void mavlink_wpm_send_waypoint_reached(uint16_t seq);
+	void			mavlink_waypoint_eventloop(uint64_t now);
+	void 			mavlink_wpm_send_waypoint_reached(uint16_t seq);
 	void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
 	void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
 	void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
@@ -354,8 +372,6 @@ private:
 
 	int message_buffer_is_empty();
 
-	bool message_buffer_write(void *ptr, int size);
-
 	int message_buffer_get_ptr(void **ptr, bool *is_part);
 
 	void message_buffer_mark_read(int n);

From a103fef948b7f239afef21a8d0f848151891b409 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 8 Jun 2014 18:51:35 +0200
Subject: [PATCH 11/61] Fixed threading and transmission issues for FTP

---
 src/modules/mavlink/mavlink_ftp.cpp      |  4 +-
 src/modules/mavlink/mavlink_ftp.h        | 33 ++++++++++---
 src/modules/mavlink/mavlink_main.cpp     | 62 +++++++++++++++++++-----
 src/modules/mavlink/mavlink_receiver.cpp |  2 +-
 4 files changed, 80 insertions(+), 21 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index d4d659d91b..8c29043e07 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -64,7 +64,7 @@ MavlinkFTP::MavlinkFTP()
 }
 
 void
-MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
+MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
 {
 	// get a free request
 	auto req = _dqFree();
@@ -73,7 +73,7 @@ MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
 	if (req != nullptr) {
 
 		// decode the request
-		if (req->decode(msg, channel)) {
+		if (req->decode(mavlink, msg)) {
 
 			// and queue it for the worker
 			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index f68dab98d5..0869a5fdbe 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -64,8 +64,8 @@ public:
 	static MavlinkFTP	*getServer();
 
 	// static interface
-	void			handle_message(mavlink_message_t *msg,
-					       mavlink_channel_t channel);
+	void			handle_message(Mavlink* mavlink,
+					mavlink_message_t *msg);
 
 private:
 
@@ -145,9 +145,9 @@ private:
 			work_s		work;
 		};
 
-		bool		decode(mavlink_message_t *fromMessage, mavlink_channel_t fromChannel) {
+		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
 			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
-				_channel = fromChannel;
+				_mavlink = mavlink;
 				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
 				return true;
 			}
@@ -155,7 +155,26 @@ private:
 		}
 
 		void		reply() {
-			mavlink_msg_encapsulated_data_send(_channel, sequence(), rawData());
+
+			// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
+			// flat memory architecture, as we're operating between threads here.
+			mavlink_message_t msg;
+			msg.checksum = 0;
+			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+				_mavlink->get_channel(), &msg, sequence(), rawData());
+			// unsigned len = mavlink_msg_system_time_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+			// 	_mavlink->get_channel(), &msg, 255, 255);
+
+			if (!_mavlink->message_buffer_write(&msg, len+2)) {
+				warnx("FTP TX ERR");
+			} else {
+				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+					_mavlink->get_system_id(),
+					_mavlink->get_component_id(),
+					_mavlink->get_channel(),
+					len,
+					msg.checksum);
+			}
 		}
 
 		uint8_t		*rawData() { return &_message.data[0]; }
@@ -163,12 +182,12 @@ private:
 		uint8_t         *requestData() { return &(header()->data[0]); }
 		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
 		uint16_t	sequence() const { return _message.seqnr; }
-		mavlink_channel_t &channel() { return _channel; }
+		mavlink_channel_t channel() { return _mavlink->get_channel(); }
 
 		char		*dataAsCString();
 
 	private:
-		mavlink_channel_t	_channel;
+		Mavlink			*_mavlink;
 		mavlink_encapsulated_data_t _message;
 
 	};
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e300be0747..066d25bf6c 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -83,6 +83,10 @@
 #include "mavlink_rate_limiter.h"
 #include "mavlink_commands.h"
 
+#ifndef MAVLINK_CRC_EXTRA
+ #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
+#endif
+
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0};
 void
 mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
 {
+
 	Mavlink *instance;
 
 	switch (channel) {
@@ -198,7 +203,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 
 		ssize_t ret = write(uart, ch, desired);
 		if (ret != desired) {
-			warnx("TX FAIL");
+			// XXX overflow perf
 		} else {
 			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
 		}
@@ -230,6 +235,7 @@ Mavlink::Mavlink() :
 	_verbose(false),
 	_forwarding_on(false),
 	_passing_on(false),
+	_ftp_on(false),
 	_uart_fd(-1),
 	_mavlink_param_queue_index(0),
 	_subscribe_to_stream(nullptr),
@@ -453,7 +459,7 @@ Mavlink::get_instance_id()
 	return _instance_id;
 }
 
-mavlink_channel_t
+const mavlink_channel_t
 Mavlink::get_channel()
 {
 	return _channel;
@@ -536,6 +542,16 @@ void Mavlink::mavlink_update_system(void)
 	_use_hil_gps = (bool)use_hil_gps;
 }
 
+int Mavlink::get_system_id()
+{
+	return mavlink_system.sysid;
+}
+
+int Mavlink::get_component_id()
+{
+	return mavlink_system.compid;
+}
+
 int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
 {
 	/* process baud rate */
@@ -1649,11 +1665,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 int
 Mavlink::message_buffer_init(int size)
 {
+
 	_message_buffer.size = size;
 	_message_buffer.write_ptr = 0;
 	_message_buffer.read_ptr = 0;
 	_message_buffer.data = (char*)malloc(_message_buffer.size);
-	return (_message_buffer.data == 0) ? ERROR : OK;
+
+	int ret;
+	if (_message_buffer.data == 0) {
+		ret = ERROR;
+		_message_buffer.size = 0;
+	} else {
+		ret = OK;
+	}
+
+	return ret;
 }
 
 void
@@ -1781,7 +1807,7 @@ Mavlink::task_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;
 
-	while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1837,6 +1863,10 @@ Mavlink::task_main(int argc, char *argv[])
 			_wait_to_transmit = true;
 			break;
 
+		case 'x':
+			_ftp_on = true;
+			break;
+
 		default:
 			err_flag = true;
 			break;
@@ -1902,9 +1932,9 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_logbuffer_init(&_logbuffer, 5);
 
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
-	if (_passing_on) {
+	if (_passing_on || _ftp_on) {
 		/* initialize message buffer if multiplexing is on */
-		if (OK != message_buffer_init(500)) {
+		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN)) {
 			errx(1, "can't allocate message buffer, exiting");
 		}
 
@@ -2064,8 +2094,8 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
-		/* pass messages from other UARTs */
-		if (_passing_on) {
+		/* pass messages from other UARTs or FTP worker */
+		if (_passing_on || _ftp_on) {
 
 			bool is_part;
 			void *read_ptr;
@@ -2076,11 +2106,21 @@ Mavlink::task_main(int argc, char *argv[])
 			pthread_mutex_unlock(&_message_buffer_mutex);
 
 			if (available > 0) {
+
+				// int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq;
+				
+				const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr;
 				/* write first part of buffer */
-				_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+				_mavlink_resend_uart(_channel, msg);
+
+				// mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq;
+				// mavlink_msg_system_time_send(get_channel(), 255, 255);
+
 				message_buffer_mark_read(available);
 
+
 				/* write second part of buffer if there is some */
+				// XXX this doesn't quite work, as the resend UART call assumes a continous block
 				if (is_part) {
 					/* guard get ptr by mutex */
 					pthread_mutex_lock(&_message_buffer_mutex);
@@ -2139,7 +2179,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* close mavlink logging device */
 	close(_mavlink_fd);
 
-	if (_passing_on) {
+	if (_passing_on || _ftp_on) {
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
@@ -2281,7 +2321,7 @@ Mavlink::stream(int argc, char *argv[])
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4f2c4ca85c..4a244815a0 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -154,7 +154,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		break;
 
 	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
-		MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel());
+		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
 		break;
 
 	default:

From 0ef66054988d8ef8053b4f64fee6454fefc04aef Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 8 Jun 2014 10:13:55 -0700
Subject: [PATCH 12/61] Fix packing of directory entries in the List reply

---
 src/modules/mavlink/mavlink_ftp.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 8c29043e07..bfee17342a 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -242,6 +242,7 @@ MavlinkFTP::_workList(Request *req)
 
 		// copy the name, which we know will fit
 		strcpy((char *)&hdr->data[offset], entry.d_name);
+		offset += strlen(entry.d_name) + 1;
 		printf("FTP: list %s\n", entry.d_name);
 	}
 

From 6d8dfd78f11fdff038f7f7504c507d25429db195 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 14 Jun 2014 15:54:24 +0200
Subject: [PATCH 13/61] sdlog2/mtecs: fix length of field name

---
 src/modules/sdlog2/sdlog2_messages.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index c42ff0afec..5645ebcf19 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -431,7 +431,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "fffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
+	LOG_FORMAT(TECS, "fffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */

From 0f41d8b5077856cf8d00f30acf9f4bc52f6e41a6 Mon Sep 17 00:00:00 2001
From: Darryl Taylor <darryl.c.taylor@gmail.com>
Date: Wed, 18 Jun 2014 13:43:03 +0800
Subject: [PATCH 14/61] Added pitch, roll, and yaw offsets to compensate for
 imperfect fmu placement. These were removed in
 61a3177979838649af2a6e8e50bea7d15e2765f4

---
 .../attitude_estimator_ekf_main.cpp                      | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 66a949af15..95685b4077 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -512,10 +512,11 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					/* send out */
 					att.timestamp = raw.timestamp;
-
-					att.roll = euler[0];
-					att.pitch = euler[1];
-					att.yaw = euler[2] + ekf_params.mag_decl;
+					
+					/* Compensate for fmu placement offsets and magnetic declination */
+					att.roll = euler[0] - ekf_params.roll_off;
+					att.pitch = euler[1] - ekf_params.pitch_off;
+					att.yaw = euler[2] + ekf_params.mag_decl - ekf_params.yaw_off;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];

From 90a89ff2cdbb5268f4b8bee64b8da24db508f5a7 Mon Sep 17 00:00:00 2001
From: Darryl Taylor <darryl.c.taylor@gmail.com>
Date: Wed, 18 Jun 2014 20:27:35 +0800
Subject: [PATCH 15/61] WIP: Support in-flight fine tuning of board alignment.
 Implemented by applying a user supplied rotation matrix via new
 SENS_BOARD_(PITCH, ROLL, YAW)_OFF params in the sensors app. Currently only
 tested in QGC.

---
 src/modules/sensors/sensor_params.c | 24 ++++++++++++++++++++++++
 src/modules/sensors/sensors.cpp     | 26 +++++++++++++++++++++++---
 2 files changed, 47 insertions(+), 3 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 999cf8bb3a..c906338227 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -242,6 +242,30 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
  */
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 
+/**
+ * Board rotation pitch offset
+ *
+ * This parameter defines a pitch offset from the board rotation. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ */
+ PARAM_DEFINE_FLOAT(SENS_BOARD_PITCH_OFF, 0);
+
+/**
+ * Board rotation roll offset
+ *
+ * This parameter defines a pitch offset from the board rotation. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_ROLL_OFF, 0);
+
+/**
+ * Board rotation YAW offset
+ *
+ * This parameter defines a pitch offset from the board rotation. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_YAW_OFF, 0);
+
 /**
  * External magnetometer rotation
  *
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b268b1b36b..1a6202d7d8 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -230,6 +230,8 @@ private:
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
 
+	math::Matrix<3, 3> _board_rotation_offset; /**< rotation matrix for fine tuning board offset **/
+	
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -252,6 +254,8 @@ private:
 
 		int board_rotation;
 		int external_mag_rotation;
+		
+		float board_offset[3];
 
 		int rc_map_roll;
 		int rc_map_pitch;
@@ -341,6 +345,8 @@ private:
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
+		
+		param_t board_offset[3];
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -587,6 +593,11 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+	
+	/* rotation offsets */
+	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF");
+	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF");
+	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -791,6 +802,15 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+	
+	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
+	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
+	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
+	
+	_board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
+
 
 	return OK;
 }
@@ -970,7 +990,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
-		vect = _board_rotation * vect;
+		vect = _board_rotation * _board_rotation_offset * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
 		raw.accelerometer_m_s2[1] = vect(1);
@@ -996,7 +1016,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-		vect = _board_rotation * vect;
+		vect = _board_rotation * _board_rotation_offset * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
 		raw.gyro_rad_s[1] = vect(1);
@@ -1027,7 +1047,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			vect = _external_mag_rotation * vect;
 
 		} else {
-			vect = _board_rotation * vect;
+			vect = _board_rotation * _board_rotation_offset * vect;
 		}
 
 		raw.magnetometer_ga[0] = vect(0);

From 95003c6e1061b61dce6ed86e6c68d662e39a222d Mon Sep 17 00:00:00 2001
From: Darryl Taylor <darryl.c.taylor@gmail.com>
Date: Wed, 18 Jun 2014 20:49:48 +0800
Subject: [PATCH 16/61] Removed unused ATT_XXX_OFF3 paramters. Offset tuning
 now accomplished via the sensors app using new SENS_BOARD_XXX_OFF parameters.

---
 .../attitude_estimator_ekf_main.cpp                 |  9 ++++-----
 .../attitude_estimator_ekf_params.c                 | 13 -------------
 .../attitude_estimator_ekf_params.h                 |  1 -
 3 files changed, 4 insertions(+), 19 deletions(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 95685b4077..66a949af15 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -512,11 +512,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					/* send out */
 					att.timestamp = raw.timestamp;
-					
-					/* Compensate for fmu placement offsets and magnetic declination */
-					att.roll = euler[0] - ekf_params.roll_off;
-					att.pitch = euler[1] - ekf_params.pitch_off;
-					att.yaw = euler[2] + ekf_params.mag_decl - ekf_params.yaw_off;
+
+					att.roll = euler[0];
+					att.pitch = euler[1];
+					att.yaw = euler[2] + ekf_params.mag_decl;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 3ff3d99229..49a8926099 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
 /* offset estimation - UNUSED */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
-/* offsets in roll, pitch and yaw of sensor plane and body */
-PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
-
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 
@@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
 	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
-	h->roll_off  =	param_find("ATT_ROLL_OFF3");
-	h->pitch_off =	param_find("ATT_PITCH_OFF3");
-	h->yaw_off   =	param_find("ATT_YAW_OFF3");
-
 	h->mag_decl   =	param_find("ATT_MAG_DECL");
 
 	h->acc_comp   =	param_find("ATT_ACC_COMP");
@@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->r2, &(p->r[2]));
 	param_get(h->r3, &(p->r[3]));
 
-	param_get(h->roll_off, &(p->roll_off));
-	param_get(h->pitch_off, &(p->pitch_off));
-	param_get(h->yaw_off, &(p->yaw_off));
-
 	param_get(h->mag_decl, &(p->mag_decl));
 	p->mag_decl *= M_PI / 180.0f;
 
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index 74a1416091..5985541cac 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params {
 struct attitude_estimator_ekf_param_handles {
 	param_t r0, r1, r2, r3;
 	param_t q0, q1, q2, q3, q4;
-	param_t roll_off, pitch_off, yaw_off;
 	param_t mag_decl;
 	param_t acc_comp;
 };

From 5ed1cf7e8d6201f1f2e0115f4941ac551f61d628 Mon Sep 17 00:00:00 2001
From: Darryl Taylor <darryl.c.taylor@gmail.com>
Date: Wed, 18 Jun 2014 21:34:38 +0800
Subject: [PATCH 17/61] Fixed too-long param names.

---
 src/modules/sensors/sensor_params.c | 10 +++++-----
 src/modules/sensors/sensors.cpp     |  6 +++---
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index c906338227..687efc49b3 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -248,23 +248,23 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
  * This parameter defines a pitch offset from the board rotation. It allows the user
  * to fine tune the board offset in the event of misalignment.
  */
- PARAM_DEFINE_FLOAT(SENS_BOARD_PITCH_OFF, 0);
+ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0);
 
 /**
  * Board rotation roll offset
  *
- * This parameter defines a pitch offset from the board rotation. It allows the user
+ * This parameter defines a roll offset from the board rotation. It allows the user
  * to fine tune the board offset in the event of misalignment.
  */
-PARAM_DEFINE_FLOAT(SENS_BOARD_ROLL_OFF, 0);
+PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0);
 
 /**
  * Board rotation YAW offset
  *
- * This parameter defines a pitch offset from the board rotation. It allows the user
+ * This parameter defines a yaw offset from the board rotation. It allows the user
  * to fine tune the board offset in the event of misalignment.
  */
-PARAM_DEFINE_FLOAT(SENS_BOARD_YAW_OFF, 0);
+PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0);
 
 /**
  * External magnetometer rotation
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 1a6202d7d8..fe741894be 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -595,9 +595,9 @@ Sensors::Sensors() :
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
 	
 	/* rotation offsets */
-	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF");
-	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF");
-	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF");
+	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
+	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
+	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
 	/* fetch initial parameter values */
 	parameters_update();

From 2475efe13d8f8d9d801276b73b98759676a64db6 Mon Sep 17 00:00:00 2001
From: Darryl Taylor <darryl.c.taylor@gmail.com>
Date: Thu, 19 Jun 2014 13:22:45 +0800
Subject: [PATCH 18/61] Pre-compute board orientation offsets on param update.

---
 src/modules/sensors/sensors.cpp | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index fe741894be..16fcb4c26d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -229,8 +229,6 @@ private:
 	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
-
-	math::Matrix<3, 3> _board_rotation_offset; /**< rotation matrix for fine tuning board offset **/
 	
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
@@ -807,10 +805,13 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
 	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
 	
-	_board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
+	/** fine tune board offset on parameter update **/
+	math::Matrix<3, 3> board_rotation_offset; 
+	board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
-
+	
+	_board_rotation = _board_rotation * board_rotation_offset;
 
 	return OK;
 }
@@ -990,7 +991,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
-		vect = _board_rotation * _board_rotation_offset * vect;
+		vect = _board_rotation * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
 		raw.accelerometer_m_s2[1] = vect(1);
@@ -1016,7 +1017,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-		vect = _board_rotation * _board_rotation_offset * vect;
+		vect = _board_rotation * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
 		raw.gyro_rad_s[1] = vect(1);
@@ -1047,7 +1048,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			vect = _external_mag_rotation * vect;
 
 		} else {
-			vect = _board_rotation * _board_rotation_offset * vect;
+			vect = _board_rotation * vect;
 		}
 
 		raw.magnetometer_ga[0] = vect(0);

From 45d5e2600911682a9117b26603e3213c6b918fa4 Mon Sep 17 00:00:00 2001
From: Darryl Taylor <darryl.c.taylor@gmail.com>
Date: Fri, 20 Jun 2014 14:37:44 +0800
Subject: [PATCH 19/61] Define float params properly: 0.0f instead of just 0

---
 src/modules/sensors/sensor_params.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 687efc49b3..850e5f4e22 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -248,7 +248,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
  * This parameter defines a pitch offset from the board rotation. It allows the user
  * to fine tune the board offset in the event of misalignment.
  */
- PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0);
+ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
 
 /**
  * Board rotation roll offset
@@ -256,7 +256,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
  * This parameter defines a roll offset from the board rotation. It allows the user
  * to fine tune the board offset in the event of misalignment.
  */
-PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0);
+PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
 
 /**
  * Board rotation YAW offset
@@ -264,7 +264,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0);
  * This parameter defines a yaw offset from the board rotation. It allows the user
  * to fine tune the board offset in the event of misalignment.
  */
-PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0);
+PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
 
 /**
  * External magnetometer rotation

From ebd68f4ffa2b6f3b6fdb1a19ef22b1aab54b4251 Mon Sep 17 00:00:00 2001
From: Darryl Taylor <darryl.c.taylor@gmail.com>
Date: Sun, 22 Jun 2014 16:43:05 +0800
Subject: [PATCH 20/61] Clarified parameter comments and added @group
 attributed for auto-generation.

---
 src/modules/sensors/sensor_params.c | 18 ++++++++++++------
 1 file changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 850e5f4e22..c8a3ec8f03 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -243,26 +243,32 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 
 /**
- * Board rotation pitch offset
+ * Board rotation Y (Pitch) offset
  *
- * This parameter defines a pitch offset from the board rotation. It allows the user
+ * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
  * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
  */
  PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
 
 /**
- * Board rotation roll offset
+ * Board rotation X (Roll) offset
  *
- * This parameter defines a roll offset from the board rotation. It allows the user
+ * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
  * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
 
 /**
- * Board rotation YAW offset
+ * Board rotation Z (YAW) offset
  *
- * This parameter defines a yaw offset from the board rotation. It allows the user
+ * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
  * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
 

From 60e93deaa3971dccba7f1e3184a69f6bafe391d8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 23 Jun 2014 07:44:30 +0200
Subject: [PATCH 21/61] phantom: silence ESC

---
 ROMFS/px4fmu_common/init.d/3031_phantom | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom
index 24372bddcf..d05c3174f9 100644
--- a/ROMFS/px4fmu_common/init.d/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -37,3 +37,7 @@ then
 fi
 
 set MIXER FMU_Q
+
+# Provide ESC a constant 1000 us pulse
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000

From cfdbf2c5e914c6264d9158470d4f98c84a483f68 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 23 Jun 2014 11:15:27 +0200
Subject: [PATCH 22/61] perfcounter: write time unit for all fields

---
 src/modules/systemlib/perf_counter.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 22182e39e8..d6d8284d24 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -301,7 +301,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
@@ -314,7 +314,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 
-		dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,

From d58a992e911114383a44327eb4478193824b580d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 23 Jun 2014 13:44:36 +0200
Subject: [PATCH 23/61] Hotfix: Improve PX4IO monitor command

---
 src/drivers/px4io/px4io.cpp          | 38 +++++++++++++++-------------
 src/drivers/px4io/px4io_uploader.cpp |  6 ++---
 2 files changed, 24 insertions(+), 20 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 972f451485..992ab9623b 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -197,8 +197,10 @@ public:
 	 * Print IO status.
 	 *
 	 * Print all relevant IO status information
+	 *
+	 * @param extended_status Shows more verbose information (in particular RC config)
 	 */
-	void			print_status();
+	void			print_status(bool extended_status);
 
 	/**
 	 * Fetch and print debug console output.
@@ -1850,7 +1852,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 }
 
 void
-PX4IO::print_status()
+PX4IO::print_status(bool extended_status)
 {
 	/* basic configuration */
 	printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
@@ -2013,19 +2015,21 @@ PX4IO::print_status()
 		printf("\n");
 	}
 
-	for (unsigned i = 0; i < _max_rc_input; i++) {
-		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
-		uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
-		printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
-		       i,
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
-		       options,
-		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
-		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+	if (extended_status) {
+		for (unsigned i = 0; i < _max_rc_input; i++) {
+			unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+			uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
+			printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
+			       i,
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
+			       options,
+			       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
+			       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+		}
 	}
 
 	printf("failsafe");
@@ -2853,7 +2857,7 @@ monitor(void)
 		if (g_dev != nullptr) {
 
 			printf("\033[2J\033[H"); /* move cursor home and clear screen */
-			(void)g_dev->print_status();
+			(void)g_dev->print_status(false);
 			(void)g_dev->print_debug();
 			printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
 
@@ -3119,7 +3123,7 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 
 		printf("[px4io] loaded\n");
-		g_dev->print_status();
+		g_dev->print_status(true);
 
 		exit(0);
 	}
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index 28ec62356b..7b6361a7ca 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -240,9 +240,9 @@ PX4IO_Uploader::upload(const char *filenames[])
 	close(_io_fd);
 	_io_fd = -1;
 
-        // sleep for enough time for the IO chip to boot. This makes
-        // forceupdate more reliably startup IO again after update
-        up_udelay(100*1000);
+	// sleep for enough time for the IO chip to boot. This makes
+	// forceupdate more reliably startup IO again after update
+	up_udelay(100*1000);
 
 	return ret;
 }

From 87857cdd48d43a28c3b8ed1f1fe500ad28a93bbc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 23 Jun 2014 13:45:20 +0200
Subject: [PATCH 24/61] Hotfix: Fix message name typo

---
 src/modules/mavlink/mavlink_messages.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index aff1aa929c..fed2dfb0d9 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1625,7 +1625,7 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
 	new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
 	new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
+	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
 	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
 	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
 	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),

From a9653fa10db3884d3d17ee33f80f23aa2e3ef842 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 23 Jun 2014 13:51:05 +0200
Subject: [PATCH 25/61] Hotfix: Only orb_copy items in mavlink app if the
 timestamp changed

---
 .../mavlink/mavlink_orb_subscription.cpp      | 26 +++++++++++--------
 1 file changed, 15 insertions(+), 11 deletions(-)

diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 901fa8f057..3807323c2a 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -44,6 +44,8 @@
 #include <uORB/uORB.h>
 #include <stdio.h>
 
+#include <systemlib/err.h>
+
 #include "mavlink_orb_subscription.h"
 
 MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
@@ -77,21 +79,23 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 		time_topic = 0;
 	}
 
-	if (orb_copy(_topic, _fd, data)) {
-		/* error copying topic data */
-		memset(data, 0, _topic->o_size);
-		return false;
+	if (time_topic != *time) {
 
-	} else {
-		/* data copied successfully */
-		_published = true;
-		if (time_topic != *time) {
-			*time = time_topic;
-			return true;
+		if (orb_copy(_topic, _fd, data)) {
+			/* error copying topic data */
+			memset(data, 0, _topic->o_size);
+			//warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size);
+			return false;
 
 		} else {
-			return false;
+			/* data copied successfully */
+			_published = true;
+			*time = time_topic;
+			return true;
 		}
+
+	} else {
+		return false;
 	}
 }
 

From bf5061aa21872c98576d46aee894e670ce0c52a0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 23 Jun 2014 13:52:09 +0200
Subject: [PATCH 26/61] Fix error reporting in stream config, report if a
 stream was not found at all in stream list and return error

---
 src/modules/mavlink/mavlink_main.cpp | 38 +++++++++++++++-------------
 1 file changed, 21 insertions(+), 17 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 046f45bd9c..a9b8323f34 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1555,32 +1555,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 				/* delete stream */
 				LL_DELETE(_streams, stream);
 				delete stream;
+				warnx("deleted stream %s", stream->get_name());
 			}
 
 			return OK;
 		}
 	}
 
-	if (interval > 0) {
-		/* search for stream with specified name in supported streams list */
-		for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
-
-			if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
-				/* create new instance */
-				stream = streams_list[i]->new_instance();
-				stream->set_channel(get_channel());
-				stream->set_interval(interval);
-				stream->subscribe(this);
-				LL_APPEND(_streams, stream);
-				return OK;
-			}
-		}
-
-	} else {
-		/* stream not found, nothing to disable */
+	if (interval == 0) {
+		/* stream was not active and is requested to be disabled, do nothing */
 		return OK;
 	}
 
+	/* search for stream with specified name in supported streams list */
+	for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+
+		if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
+			/* create new instance */
+			stream = streams_list[i]->new_instance();
+			stream->set_channel(get_channel());
+			stream->set_interval(interval);
+			stream->subscribe(this);
+			LL_APPEND(_streams, stream);
+
+			return OK;
+		}
+	}
+
+	/* if we reach here, the stream list does not contain the stream */
+	warnx("stream %s not found", stream_name);
+
 	return ERROR;
 }
 

From 68442e31ac6970be91592282c9b70ebc76fa142d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 23 Jun 2014 14:56:48 +0200
Subject: [PATCH 27/61] Hotfix: Move channel count to right position

---
 src/modules/mavlink/mavlink_messages.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index fed2dfb0d9..4433377c6d 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1275,6 +1275,7 @@ protected:
 			// New message
 			mavlink_msg_rc_channels_send(_channel,
 					rc.timestamp_publication / 1000,
+					rc.channel_count,
 					((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
 					((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
 					((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
@@ -1293,7 +1294,6 @@ protected:
 					((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
 					((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
 					((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
-					rc.channel_count,
 					rc.rssi);
 		}
 	}

From 52d539d3cdf56eb4145dbf29c407bb0a1a7eebe5 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 24 Jun 2014 17:39:48 +0200
Subject: [PATCH 28/61] extend integrator reset flags in uorb attitude setpoint
 topic to all axes

---
 src/modules/uORB/topics/vehicle_attitude_setpoint.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index d526a8ff27..8446e9c6e2 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s {
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
 	bool	roll_reset_integral;			/**< Reset roll integral part (navigation logic change) */
+	bool	pitch_reset_integral;			/**< Reset pitch integral part (navigation logic change) */
+	bool	yaw_reset_integral;			/**< Reset yaw integral part (navigation logic change) */
 
 };
 

From 9a911f7388e1a48630469fd2e875f00a119c829a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 24 Jun 2014 17:40:56 +0200
Subject: [PATCH 29/61] fw att control: reset integrators when requested via
 attitude setpoint topic

---
 src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 178b590ae5..3cd4ce9285 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main()
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
+					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
 					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
-					if (_att_sp.roll_reset_integral)
+					if (_att_sp.roll_reset_integral) {
 						_roll_ctrl.reset_integrator();
-
+					}
+					if (_att_sp.pitch_reset_integral) {
+						_pitch_ctrl.reset_integrator();
+					}
+					if (_att_sp.yaw_reset_integral) {
+						_yaw_ctrl.reset_integrator();
+					}
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians

From c6cdcfc2638f8d994b3716d8739614c5377c4962 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 24 Jun 2014 17:42:21 +0200
Subject: [PATCH 30/61] fw pos control: set integrator reset flags in attitude
 setpoint topic, set them to true when launchdetection is running (while
 waiting for launch)

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 116d3cc63d..518116fa1d 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -841,6 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* current waypoint (the one currently heading for) */
 		math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
+		/* Initialize attitude controller integrator reset flags to 0 */
+		_att_sp.roll_reset_integral = false;
+		_att_sp.pitch_reset_integral = false;
+		_att_sp.yaw_reset_integral = false;
 
 		/* previous waypoint */
 		math::Vector<2> prev_wp;
@@ -1028,6 +1032,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 //						warnx("Launch detection running");
 						mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
 						last_sent = hrt_absolute_time();
+						/* Tell the attitude controller to stop integrating while we are
+						 * waiting for the launch */
+						_att_sp.roll_reset_integral = true;
+						_att_sp.pitch_reset_integral = true;
+						_att_sp.yaw_reset_integral = true;
 					}
 					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
 					if (launchDetector.getLaunchDetected()) {

From 819812e11271d7b78f4af2d5bbb7ac6e4be9e494 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 24 Jun 2014 20:52:24 +0200
Subject: [PATCH 31/61] fw pos ctrl: move setting of attitude integral reset
 flag

---
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp    | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 518116fa1d..fe58476821 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1032,12 +1032,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 //						warnx("Launch detection running");
 						mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
 						last_sent = hrt_absolute_time();
-						/* Tell the attitude controller to stop integrating while we are
-						 * waiting for the launch */
-						_att_sp.roll_reset_integral = true;
-						_att_sp.pitch_reset_integral = true;
-						_att_sp.yaw_reset_integral = true;
 					}
+
+					/* Tell the attitude controller to stop integrating while we are waiting
+					 * for the launch */
+					_att_sp.roll_reset_integral = true;
+					_att_sp.pitch_reset_integral = true;
+					_att_sp.yaw_reset_integral = true;
+
+					/* Detect launch */
 					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
 					if (launchDetector.getLaunchDetected()) {
 						launch_detected = true;

From 4f560f729ec1f40427401119a0f17d9d0e9843f4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 24 Jun 2014 20:53:06 +0200
Subject: [PATCH 32/61] fw pos ctrl: remove comments

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index fe58476821..0e065211e9 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1024,12 +1024,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
 
 			/* Perform launch detection */
-//			warnx("Launch detection running");
 			if(!launch_detected) { //do not do further checks once a launch was detected
 				if (launchDetector.launchDetectionEnabled()) {
 					static hrt_abstime last_sent = 0;
 					if(hrt_absolute_time() - last_sent > 4e6) {
-//						warnx("Launch detection running");
 						mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
 						last_sent = hrt_absolute_time();
 					}

From f9d5cf332c9e60661c2e1c0827c84480e49d4fe8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 25 Jun 2014 17:05:20 +0200
Subject: [PATCH 33/61] Revert "Hotfix: Only orb_copy items in mavlink app if
 the timestamp changed"

This reverts commit a9653fa10db3884d3d17ee33f80f23aa2e3ef842.
---
 .../mavlink/mavlink_orb_subscription.cpp      | 30 ++++++++-----------
 1 file changed, 13 insertions(+), 17 deletions(-)

diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 3807323c2a..901fa8f057 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -44,8 +44,6 @@
 #include <uORB/uORB.h>
 #include <stdio.h>
 
-#include <systemlib/err.h>
-
 #include "mavlink_orb_subscription.h"
 
 MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
@@ -79,23 +77,21 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 		time_topic = 0;
 	}
 
-	if (time_topic != *time) {
-
-		if (orb_copy(_topic, _fd, data)) {
-			/* error copying topic data */
-			memset(data, 0, _topic->o_size);
-			//warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size);
-			return false;
-
-		} else {
-			/* data copied successfully */
-			_published = true;
-			*time = time_topic;
-			return true;
-		}
+	if (orb_copy(_topic, _fd, data)) {
+		/* error copying topic data */
+		memset(data, 0, _topic->o_size);
+		return false;
 
 	} else {
-		return false;
+		/* data copied successfully */
+		_published = true;
+		if (time_topic != *time) {
+			*time = time_topic;
+			return true;
+
+		} else {
+			return false;
+		}
 	}
 }
 

From 9d3d5a30af0781b975d17c5d73796f593c78b6e4 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 25 Jun 2014 17:13:47 +0200
Subject: [PATCH 34/61] navigator: move set_previous_pos_sp to MissionBlock
 class

---
 src/modules/navigator/mission.cpp       | 12 +-----------
 src/modules/navigator/mission.h         |  6 ------
 src/modules/navigator/mission_block.cpp |  9 +++++++++
 src/modules/navigator/mission_block.h   |  5 +++++
 4 files changed, 15 insertions(+), 17 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 9244063b19..becaa09e83 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -223,7 +223,7 @@ Mission::advance_mission()
 void
 Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
+	set_previous_pos_setpoint(pos_sp_triplet);
 
 	/* try setting onboard mission item */
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -261,16 +261,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 	}
 }
 
-void
-Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-		                   struct position_setpoint_s *previous_pos_sp)
-{
-	/* reuse current setpoint as previous setpoint */
-	if (current_pos_sp->valid) {
-		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
-	}
-}
-
 bool
 Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 322aaf96af..842c8c9ee8 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -106,12 +106,6 @@ private:
 	 */
 	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	/**
-	 * Set previous position setpoint
-	 */
-	void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-			               struct position_setpoint_s *previous_pos_sp);
-
 	/**
 	 * Try to set the current position setpoint from an onboard mission item
 	 * @return true if mission item successfully set
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 08576750c9..a8eec92ca6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -194,6 +194,15 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	}
 }
 
+void
+MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+    /* reuse current setpoint as previous setpoint */
+    if (pos_sp_triplet->current.valid) {
+        memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
+    }
+}
+
 bool
 MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
 {
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 47e6fe81fe..a740deea48 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -82,6 +82,11 @@ public:
 	 */
 	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
+    /**
+     * Set previous position setpoint to current setpoint
+     */
+	void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
+
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
 	 *

From bdf1b9274c369ddfd52bcb92952f2467abd7c26f Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 25 Jun 2014 17:56:11 +0200
Subject: [PATCH 35/61] commander: modes fallback and reject messages fixed

---
 src/modules/commander/commander.cpp | 30 +++++++++++++++++++++++++----
 1 file changed, 26 insertions(+), 4 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index b957ad9b5f..ec4273fc33 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1613,10 +1613,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 				break;	// changed successfully or already in this state
 			}
 
-			// else fallback to ALTCTL
 			print_reject_mode(status, "POSCTL");
 		}
 
+        // fallback to ALTCTL
 		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
@@ -1627,7 +1627,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 			print_reject_mode(status, "ALTCTL");
 		}
 
-		// else fallback to MANUAL
+		// fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
@@ -1639,28 +1639,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}
+
+            print_reject_mode(status, "AUTO_RTL");
+
+            // fallback to LOITER if home position not set
+            res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
+
+            if (res != TRANSITION_DENIED) {
+                break;  // changed successfully or already in this state
+            }
+
 		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}
+
+            print_reject_mode(status, "AUTO_LOITER");
+
 		} else {
 			res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}
+
+            print_reject_mode(status, "AUTO_MISSION");
 		}
 
-		// else fallback to ALTCTL (POSCTL likely will not work too)
+        // fallback to POSCTL
+        res = main_state_transition(status, MAIN_STATE_POSCTL);
+
+        if (res != TRANSITION_DENIED) {
+            break;  // changed successfully or already in this state
+        }
+
+		// fallback to ALTCTL
 		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
 		}
 
-		// else fallback to MANUAL
+		// fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;

From 39454ca99d984698983404b2c82bbd61f93be3fc Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 25 Jun 2014 17:56:30 +0200
Subject: [PATCH 36/61] navigator: RTL return altitude fixed

---
 src/modules/navigator/rtl.cpp | 59 ++++++++++++++++++-----------------
 1 file changed, 31 insertions(+), 28 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c1b1d3f097..46194ed52a 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -44,6 +44,7 @@
 
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -112,21 +113,29 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		if (_navigator->get_vstatus()->condition_landed) {
 			_rtl_state = RTL_STATE_FINISHED;
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+
 		/* if lower than return altitude, climb up first */
 		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
 			   + _param_return_alt.get()) {
 			_rtl_state = RTL_STATE_CLIMB;
+
 		/* otherwise go straight to return */
 		} else {
+		    /* set altitude setpoint to current altitude */
 			_rtl_state = RTL_STATE_RETURN;
+	        _mission_item.altitude_is_relative = false;
+	        _mission_item.altitude = _navigator->get_global_position()->alt;
 		}
 	}
 
-	/* if switching directly to return state, set altitude setpoint to current altitude */
-	if (_rtl_state == RTL_STATE_RETURN) {
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_global_position()->alt;
-	}
+    if (_rtl_state == RTL_STATE_FINISHED) {
+        /* RTL finished, nothing to do */
+        pos_sp_triplet->current.valid = false;
+        pos_sp_triplet->next.valid = false;
+        return;
+    }
+
+    set_previous_pos_setpoint(pos_sp_triplet);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -157,17 +166,20 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
+		 // don't change altitude
 
-		/* TODO: add this again */
-		// don't change altitude
-		// if (_pos_sp_triplet.previous.valid) {
-		// 	/* if previous setpoint is valid then use it to calculate heading to home */
-		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
+		 if (pos_sp_triplet->previous.valid) {
+		 	/* if previous setpoint is valid then use it to calculate heading to home */
+		 	_mission_item.yaw = get_bearing_to_next_waypoint(
+		 	        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
+		 	        _mission_item.lat, _mission_item.lon);
 
-		// } else {
-		// 	/* else use current position */
-		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
-		// }
+		 } else {
+		 	/* else use current position */
+		 	_mission_item.yaw = get_bearing_to_next_waypoint(
+		 	        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+		 	        _mission_item.lat, _mission_item.lon);
+		 }
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -228,24 +240,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-	case RTL_STATE_FINISHED: {
-		/* nothing to do, report fail */
-	}
-
 	default:
 		break;
 	}
 
-	if (_rtl_state == RTL_STATE_FINISHED) {
-		pos_sp_triplet->current.valid = false;
-		pos_sp_triplet->next.valid = false;
-	} else {
-		/* if not finished, convert mission item to current position setpoint and make it valid */
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-		reset_mission_item_reached();
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->next.valid = false;
-	}
+    /* convert mission item to current position setpoint and make it valid */
+    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+    reset_mission_item_reached();
+    pos_sp_triplet->current.valid = true;
+    pos_sp_triplet->next.valid = false;
 }
 
 void

From c5a5604ae975294347f1571258a633c11ef61261 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 00:17:25 +0200
Subject: [PATCH 37/61] navigator: loiter fixes

---
 src/modules/navigator/loiter.cpp         |  3 +-
 src/modules/navigator/mission.cpp        |  8 ++---
 src/modules/navigator/mission_block.cpp  | 40 +++++++++++++-----------
 src/modules/navigator/mission_block.h    |  3 +-
 src/modules/navigator/navigator.h        |  6 ++--
 src/modules/navigator/navigator_main.cpp |  4 +--
 src/modules/navigator/rtl.cpp            |  9 +-----
 7 files changed, 34 insertions(+), 39 deletions(-)

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 8bbb79d5ee..f5eb1e7a5a 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -70,11 +70,10 @@ bool
 Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	/* set loiter item, don't reuse an existing position setpoint */
-	return set_loiter_item(false, pos_sp_triplet);;
+	return set_loiter_item(pos_sp_triplet);
 }
 
 void
 Loiter::on_inactive()
 {
 }
-
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index becaa09e83..c09f617446 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -233,7 +233,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: onboard mission running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_navigator->set_is_in_loiter(false);
+		_navigator->set_can_loiter_at_sp(false);
 
 	/* try setting offboard mission item */
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -243,7 +243,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: offboard mission running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_navigator->set_is_in_loiter(false);
+		_navigator->set_can_loiter_at_sp(false);
 	} else {
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_info(_navigator->get_mavlink_fd(),
@@ -253,9 +253,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: no mission available");
 		}
 		_mission_type = MISSION_TYPE_NONE;
+        _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
-		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
-		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
+		set_loiter_item(pos_sp_triplet);
 		reset_mission_item_reached();
 		report_mission_finished();
 	}
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index a8eec92ca6..41d81ad9b0 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -204,31 +204,35 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_
 }
 
 bool
-MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	if (_navigator_priv->get_is_in_loiter()) {
-		/* already loitering, bail out */
-		return false;
-	}
-
-	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
-		/* leave position setpoint as is */
-	} else {
+    /* don't change setpoint if 'can_loiter_at_sp' flag set */
+	if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
 		/* use current position */
 		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
 		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
 		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
 		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
+
+	    _navigator_priv->set_can_loiter_at_sp(true);
 	}
-	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-	pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
-	pos_sp_triplet->current.loiter_direction = 1;
 
-	pos_sp_triplet->previous.valid = false;
-	pos_sp_triplet->current.valid = true;
-	pos_sp_triplet->next.valid = false;
+    if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
+            || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
+            || pos_sp_triplet->current.loiter_direction != 1
+            || pos_sp_triplet->previous.valid
+            || !pos_sp_triplet->current.valid
+            || pos_sp_triplet->next.valid) {
+        /* position setpoint triplet should be updated */
+        pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+        pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+        pos_sp_triplet->current.loiter_direction = 1;
 
-	_navigator_priv->set_is_in_loiter(true);
-	return true;
+        pos_sp_triplet->previous.valid = false;
+        pos_sp_triplet->current.valid = true;
+        pos_sp_triplet->next.valid = false;
+        return true;
+    }
+
+    return false;
 }
-
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index a740deea48..8726964fa3 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -90,11 +90,10 @@ public:
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
 	 *
-	 * @param true if the current position setpoint should be re-used
 	 * @param the position setpoint triplet to set
 	 * @return true if setpoint has changed
 	 */
-	bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
+	bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
 
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index dadd155271..4709f71965 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -102,7 +102,7 @@ public:
 	/**
 	 * Setters
 	 */
-	void		set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
+	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 
 	/**
 	 * Getters
@@ -113,7 +113,7 @@ public:
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
 	Geofence&	get_geofence() { return _geofence; }
-	bool		get_is_in_loiter() { return _is_in_loiter; }
+	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
 	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
@@ -161,7 +161,7 @@ private:
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
-	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
+	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d762b8a9dc..f39300590c 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -347,7 +347,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
 				_navigation_mode = nullptr;
-				_is_in_loiter = false;
+				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
 				_navigation_mode = &_mission;
@@ -365,7 +365,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_TERMINATION:
 			default:
 				_navigation_mode = nullptr;
-				_is_in_loiter = false;
+				_can_loiter_at_sp = false;
 				break;
 		}
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 46194ed52a..411f8c5272 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -136,6 +136,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
     }
 
     set_previous_pos_setpoint(pos_sp_triplet);
+    _navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -156,8 +157,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
@@ -189,8 +188,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
@@ -211,8 +208,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(true);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
@@ -234,8 +229,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
 		break;
 	}

From 831a3d4ed14bc068ee086c5190af96ad809f67d7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 26 Jun 2014 07:47:46 +0200
Subject: [PATCH 38/61] mtecs: improve logic readability

---
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 03353cbc1a..48c9079a48 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -199,8 +199,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	}
 
 	/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
-	BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits
-	BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits
+	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
+	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
 	if (mode == TECS_MODE_TAKEOFF) {
 		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
@@ -218,12 +218,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
 	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
 	 * is running) */
-	bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
-			_controlTotalEnergy.getOutputLimiter() :
-			*outputLimiterThrottle,
-			outputLimiterPitch == NULL ?
-			_controlEnergyDistribution.getOutputLimiter() :
-			*outputLimiterPitch);
+	bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
 	/* Write part of the status message */
 	_status.airspeedDerivativeSp = airspeedDerivativeSp;

From 43c3559763841abacf5e74e15a6172026071088b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 26 Jun 2014 12:08:25 +0200
Subject: [PATCH 39/61] commander: Make mavlink_fd in arming command
 non-optional

---
 src/modules/commander/state_machine_helper.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0ddd4f05a2..abb9178732 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
 } transition_result_t;
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 

From 547f71d791d0a04001580e8371bdf59b808a3d29 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 26 Jun 2014 12:07:28 +0200
Subject: [PATCH 40/61] Add mavlink_fd to all commander arm transitions to
 provide user feedback why the arming command failed

---
 src/modules/commander/commander.cpp | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 65922b2a5d..588f48225c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			/* if HIL got enabled, reset battery status state */
 			if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
 				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd);
 
 				if (arming_res != TRANSITION_DENIED) {
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
@@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 
-				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
-					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
+					mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch");
 				}
 			}
 		}
@@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
 
 			} else {
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
 			}
 
 			status_changed = true;
@@ -1163,7 +1163,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			// XXX check for sensors
-			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
 
 		} else {
 			// XXX: Add emergency stuff if sensors are lost
@@ -1217,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
 					stick_off_counter = 0;
 
 				} else {
@@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[])
 						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
 					}
 
 					stick_on_counter = 0;
@@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg)
 				/* try to go to INIT/PREFLIGHT arming state */
 
 				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
 
 				break;
 			}

From 63e14c73bab8f5b14b5259ba249f84a8edb0ee05 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 12:18:19 +0200
Subject: [PATCH 41/61] navigator: don't reset RTL state on loiter

---
 src/modules/navigator/rtl.cpp | 58 +++++++++++++++++++----------------
 1 file changed, 31 insertions(+), 27 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 411f8c5272..a2eec85893 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -76,7 +76,11 @@ void
 RTL::on_inactive()
 {
 	_first_run = true;
-	_rtl_state = RTL_STATE_NONE;
+
+	/* reset RTL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+	    _rtl_state = RTL_STATE_NONE;
+	}
 }
 
 bool
@@ -85,14 +89,35 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	bool updated = false;
 
 	if (_first_run) {
+        _first_run = false;
+
+	    /* decide where to enter the RTL procedure when we switch into it */
+	    if (_rtl_state == RTL_STATE_NONE) {
+	        /* for safety reasons don't go into RTL if landed */
+	        if (_navigator->get_vstatus()->condition_landed) {
+	            _rtl_state = RTL_STATE_FINISHED;
+	            mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+
+	        /* if lower than return altitude, climb up first */
+	        } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+	               + _param_return_alt.get()) {
+	            _rtl_state = RTL_STATE_CLIMB;
+
+	        /* otherwise go straight to return */
+	        } else {
+	            /* set altitude setpoint to current altitude */
+	            _rtl_state = RTL_STATE_RETURN;
+	            _mission_item.altitude_is_relative = false;
+	            _mission_item.altitude = _navigator->get_global_position()->alt;
+	        }
+	    }
+
 		set_rtl_item(pos_sp_triplet);
+
 		updated = true;
-		_first_run = false;
 	}
 
-	if ((_rtl_state == RTL_STATE_CLIMB
-	     || _rtl_state == RTL_STATE_RETURN)
-	    && is_mission_item_reached()) {
+	if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) {
 		advance_rtl();
 		set_rtl_item(pos_sp_triplet);
 		updated = true;
@@ -107,31 +132,10 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-	/* decide where to enter the RTL procedure when we switch into it */
-	if (_rtl_state == RTL_STATE_NONE) {
-		/* for safety reasons don't go into RTL if landed */
-		if (_navigator->get_vstatus()->condition_landed) {
-			_rtl_state = RTL_STATE_FINISHED;
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
-
-		/* if lower than return altitude, climb up first */
-		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
-			   + _param_return_alt.get()) {
-			_rtl_state = RTL_STATE_CLIMB;
-
-		/* otherwise go straight to return */
-		} else {
-		    /* set altitude setpoint to current altitude */
-			_rtl_state = RTL_STATE_RETURN;
-	        _mission_item.altitude_is_relative = false;
-	        _mission_item.altitude = _navigator->get_global_position()->alt;
-		}
-	}
-
     if (_rtl_state == RTL_STATE_FINISHED) {
         /* RTL finished, nothing to do */
-        pos_sp_triplet->current.valid = false;
         pos_sp_triplet->next.valid = false;
+        mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
         return;
     }
 

From 6e5aafb3a753f83b1db936e94de09144ee115c0d Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 13:44:41 +0200
Subject: [PATCH 42/61] navigator: minor formatting fix

---
 src/modules/navigator/navigator_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f39300590c..493e86d4a7 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -386,7 +386,7 @@ Navigator::task_main()
 			_update_triplet = true;
 		}
 
-		if (_update_triplet ) {
+		if (_update_triplet) {
 			publish_position_setpoint_triplet();
 			_update_triplet = false;
 		}

From 1a0f53ec3a95c35106f63d58b0ca0b5e7b816d89 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 26 Jun 2014 13:50:46 +0200
Subject: [PATCH 43/61] mavlink-ftp: Remove two extra bytes we don not need

---
 src/modules/mavlink/mavlink_ftp.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 0869a5fdbe..34b67b87f7 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -165,7 +165,7 @@ private:
 			// unsigned len = mavlink_msg_system_time_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
 			// 	_mavlink->get_channel(), &msg, 255, 255);
 
-			if (!_mavlink->message_buffer_write(&msg, len+2)) {
+			if (!_mavlink->message_buffer_write(&msg, len)) {
 				warnx("FTP TX ERR");
 			} else {
 				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",

From f9db1723e3899589c10f4f8847c3db1029f999cb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 26 Jun 2014 14:20:29 +0200
Subject: [PATCH 44/61] mavlink-ftp: Remove commented out code

---
 src/modules/mavlink/mavlink_ftp.h | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 34b67b87f7..462594301c 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -162,8 +162,6 @@ private:
 			msg.checksum = 0;
 			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
 				_mavlink->get_channel(), &msg, sequence(), rawData());
-			// unsigned len = mavlink_msg_system_time_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
-			// 	_mavlink->get_channel(), &msg, 255, 255);
 
 			if (!_mavlink->message_buffer_write(&msg, len)) {
 				warnx("FTP TX ERR");

From 7546b99a24a33ff160b29b5be31a9e0d39bbce1a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 26 Jun 2014 14:26:35 +0200
Subject: [PATCH 45/61] mavlink-ftp: Add extra padding because the ringbuffer
 implementation relies on its use

---
 src/modules/mavlink/mavlink_main.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 647519ea59..e9cce175d9 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1903,8 +1903,11 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
 	if (_passing_on || _ftp_on) {
-		/* initialize message buffer if multiplexing is on */
-		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN)) {
+		/* initialize message buffer if multiplexing is on or its needed for FTP.
+		 * make space for two messages plus off-by-one space as we use the empty element
+		 * marker ring buffer approach.
+		 */
+		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
 			errx(1, "can't allocate message buffer, exiting");
 		}
 

From f76040e55407d3526fb7655ec732304c9ba1dc8b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 26 Jun 2014 12:54:33 +0200
Subject: [PATCH 46/61] mavlink: Always send heartbeat and do not require both
 topics to update

---
 src/modules/mavlink/mavlink_messages.cpp | 26 +++++++++++++-----------
 1 file changed, 14 insertions(+), 12 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 4433377c6d..b295bf35f8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -226,19 +226,21 @@ protected:
 		struct vehicle_status_s status;
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
-		if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_base_mode = 0;
-			uint32_t mavlink_custom_mode = 0;
-			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+		/* always send the heartbeat, independent of the update status of the topics */
+		(void)status_sub->update(&status);
+		(void)pos_sp_triplet_sub->update(&pos_sp_triplet);
 
-			mavlink_msg_heartbeat_send(_channel,
-						   mavlink_system.type,
-						   MAV_AUTOPILOT_PX4,
-						   mavlink_base_mode,
-						   mavlink_custom_mode,
-						   mavlink_state);
-		}
+		uint8_t mavlink_state = 0;
+		uint8_t mavlink_base_mode = 0;
+		uint32_t mavlink_custom_mode = 0;
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+		mavlink_msg_heartbeat_send(_channel,
+					   mavlink_system.type,
+					   MAV_AUTOPILOT_PX4,
+					   mavlink_base_mode,
+					   mavlink_custom_mode,
+					   mavlink_state);
 	}
 };
 

From 62156e78ae45e1c5e2a5ecc305b30cf1b4e0e7f2 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 22:37:11 +0200
Subject: [PATCH 47/61] mavlink: init structs for HEARTBEAT if uORB topic copy
 failed

---
 src/modules/mavlink/mavlink_messages.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b295bf35f8..97520fce90 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -227,8 +227,15 @@ protected:
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		/* always send the heartbeat, independent of the update status of the topics */
-		(void)status_sub->update(&status);
-		(void)pos_sp_triplet_sub->update(&pos_sp_triplet);
+		if (!status_sub->update(&status)) {
+            /* if topic update failed fill it with defaults */
+            memset(&status, 0, sizeof(status));
+		}
+
+		if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+            /* if topic update failed fill it with defaults */
+            memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+		}
 
 		uint8_t mavlink_state = 0;
 		uint8_t mavlink_base_mode = 0;

From 9e5c94b41e6e4d0b5869c80ee4f6248bedffe7b4 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 22:41:35 +0200
Subject: [PATCH 48/61] mavlink: spaces replaced with tabs

---
 src/modules/mavlink/mavlink_messages.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 97520fce90..ef9039a6ab 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -228,13 +228,13 @@ protected:
 
 		/* always send the heartbeat, independent of the update status of the topics */
 		if (!status_sub->update(&status)) {
-            /* if topic update failed fill it with defaults */
-            memset(&status, 0, sizeof(status));
+			/* if topic update failed fill it with defaults */
+			memset(&status, 0, sizeof(status));
 		}
 
 		if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-            /* if topic update failed fill it with defaults */
-            memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+			/* if topic update failed fill it with defaults */
+			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
 		}
 
 		uint8_t mavlink_state = 0;

From 8e8798a5225d333596c9018ec703da0c6787493d Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 23:39:17 +0200
Subject: [PATCH 49/61] navigator: spaces/tabs fixed, old commented code
 removed

---
 src/modules/navigator/navigator_main.cpp | 322 -----------------------
 src/modules/navigator/rtl.cpp            |  66 ++---
 2 files changed, 33 insertions(+), 355 deletions(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 493e86d4a7..e22c4d72df 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -446,329 +446,7 @@ Navigator::status()
 		warnx("Geofence not set");
 	}
 }
-#if 0
-bool
-Navigator::start_none_on_ground()
-{
-	reset_reached();
 
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_none_in_air()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_auto_on_ground()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = true;
-	_pos_sp_triplet.next.valid = false;
-
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_loiter()
-{
-	/* if no existing item available, use current position */
-	if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
-
-		_pos_sp_triplet.current.lat = _global_pos.lat;
-		_pos_sp_triplet.current.lon = _global_pos.lon;
-		_pos_sp_triplet.current.yaw = NAN;	// NAN means to use current yaw
-		_pos_sp_triplet.current.alt = _global_pos.alt;
-	}
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
-	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
-	_pos_sp_triplet.current.loiter_direction = 1;
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = true;
-	_pos_sp_triplet.next.valid = false;
-
-	mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_mission()
-{
-	/* start fresh */
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	return set_mission_items();
-}
-
-bool
-Navigator::advance_mission()
-{
-	/* tell mission to move by one */
-	_mission.move_to_next();
-
-	/* now try to set the new mission items, if it fails, it will dispatch loiter */
-	return set_mission_items();
-}
-
-bool
-Navigator::set_mission_items()
-{
-	if (_pos_sp_triplet.current.valid) {
-		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-		_pos_sp_triplet.previous.valid = true;
-	}
-
-	bool onboard;
-	int index;
-
-	/* if we fail to set the current mission, continue to loiter */
-	if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
-
-		return false;
-	}
-
-	/* if we got an RTL mission item, switch to RTL mode and give up */
-	if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		return false;
-	}
-
-	_mission_item_valid = true;
-
-	/* convert the current mission item and set it valid */
-	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-	_pos_sp_triplet.current.valid = true;
-
-
-	mission_item_s next_mission_item;
-
-	bool last_wp = false;
-	/* now try to set the next mission item as well, if there is no more next
-	 * this means we're heading to the last waypoint */
-	if (_mission.get_next_mission_item(&next_mission_item)) {
-		/* convert the next mission item and set it valid */
-		mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
-		_pos_sp_triplet.next.valid = true;
-	} else {
-		last_wp = true;
-	}
-
-	/* notify user about what happened */
-	mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
-		(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
-
-	_update_triplet = true;
-
-	reset_reached();
-
-	return true;
-}
-
-bool
-Navigator::start_rtl()
-{
-	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
-
-		_mission_item_valid = true;
-
-		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-		_pos_sp_triplet.current.valid = true;
-
-		reset_reached();
-
-		_update_triplet = true;
-		return true;
-	}
-
-	/* if RTL doesn't work, fallback to loiter */
-	return false;
-}
-
-bool
-Navigator::advance_rtl()
-{
-	/* tell mission to move by one */
-	_rtl.move_to_next();
-
-	/* now try to set the new mission items, if it fails, it will dispatch loiter */
-	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
-
-		_mission_item_valid = true;
-
-		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-		_pos_sp_triplet.current.valid = true;
-
-		reset_reached();
-
-		_update_triplet = true;
-		return true;
-	}
-
-	return false;
-}
-
-bool
-Navigator::start_land()
-{
-	/* TODO: verify/test */
-
-	reset_reached();
-
-	/* this state can be requested by commander even if no global position available,
-	 * in his case controller must perform landing without position control */
-
-	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-	_mission_item.lat = _global_pos.lat;
-	_mission_item.lon = _global_pos.lon;
-	_mission_item.altitude_is_relative = false;
-	_mission_item.altitude = _global_pos.alt;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _parameters.loiter_radius;
-	_mission_item.loiter_direction = 1;
-	_mission_item.nav_cmd = NAV_CMD_LAND;
-	_mission_item.acceptance_radius = _parameters.acceptance_radius;
-	_mission_item.time_inside = 0.0f;
-	_mission_item.pitch_min = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
-
-	_mission_item_valid = true;
-
-	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-bool
-Navigator::check_mission_item_reached()
-{
-	/* only check if there is actually a mission item to check */
-	if (!_mission_item_valid) {
-		return false;
-	}
-
-	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return _vstatus.condition_landed;
-	}
-
-	/* XXX TODO count turns */
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
-	// 	return false;
-	// }
-
-	uint64_t now = hrt_absolute_time();
-
-	if (!_waypoint_position_reached) {
-		float acceptance_radius;
-
-		if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
-			acceptance_radius = _mission_item.acceptance_radius;
-
-		} else {
-			acceptance_radius = _parameters.acceptance_radius;
-		}
-
-		if (_do_takeoff) {
-			/* require only altitude for takeoff */
-			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		} else {
-			float dist = -1.0f;
-			float dist_xy = -1.0f;
-			float dist_z = -1.0f;
-
-			/* calculate AMSL altitude for this waypoint */
-			float wp_alt_amsl = _mission_item.altitude;
-
-			if (_mission_item.altitude_is_relative)
-				wp_alt_amsl += _home_pos.alt;
-
-			dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-					(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
-					&dist_xy, &dist_z);
-
-			if (dist >= 0.0f && dist <= acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		}
-	}
-
-	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-
-		/* TODO: removed takeoff, why? */
-		if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {
-
-			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
-
-			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
-				_waypoint_yaw_reached = true;
-			}
-
-		} else {
-			_waypoint_yaw_reached = true;
-		}
-	}
-
-	/* check if the current waypoint was reached */
-	if (_waypoint_position_reached && _waypoint_yaw_reached) {
-
-		if (_time_first_inside_orbit == 0) {
-			_time_first_inside_orbit = now;
-
-			if (_mission_item.time_inside > 0.01f) {
-				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
-					(double)_mission_item.time_inside);
-			}
-		}
-
-		/* check if the MAV was long enough inside the waypoint orbit */
-		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
-		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			return true;
-		}
-	}
-	return false;
-}
-
-void
-Navigator::reset_reached()
-{
-	_time_first_inside_orbit = 0;
-	_waypoint_position_reached = false;
-	_waypoint_yaw_reached = false;
-
-}
-#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index a2eec85893..c5bb06c3b7 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -79,7 +79,7 @@ RTL::on_inactive()
 
 	/* reset RTL state only if setpoint moved */
 	if (!_navigator->get_can_loiter_at_sp()) {
-	    _rtl_state = RTL_STATE_NONE;
+		_rtl_state = RTL_STATE_NONE;
 	}
 }
 
@@ -89,28 +89,28 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	bool updated = false;
 
 	if (_first_run) {
-        _first_run = false;
+		_first_run = false;
 
-	    /* decide where to enter the RTL procedure when we switch into it */
-	    if (_rtl_state == RTL_STATE_NONE) {
-	        /* for safety reasons don't go into RTL if landed */
-	        if (_navigator->get_vstatus()->condition_landed) {
-	            _rtl_state = RTL_STATE_FINISHED;
-	            mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+		/* decide where to enter the RTL procedure when we switch into it */
+		if (_rtl_state == RTL_STATE_NONE) {
+			/* for safety reasons don't go into RTL if landed */
+			if (_navigator->get_vstatus()->condition_landed) {
+				_rtl_state = RTL_STATE_FINISHED;
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
 
-	        /* if lower than return altitude, climb up first */
-	        } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
-	               + _param_return_alt.get()) {
-	            _rtl_state = RTL_STATE_CLIMB;
+			/* if lower than return altitude, climb up first */
+			} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+				   + _param_return_alt.get()) {
+				_rtl_state = RTL_STATE_CLIMB;
 
-	        /* otherwise go straight to return */
-	        } else {
-	            /* set altitude setpoint to current altitude */
-	            _rtl_state = RTL_STATE_RETURN;
-	            _mission_item.altitude_is_relative = false;
-	            _mission_item.altitude = _navigator->get_global_position()->alt;
-	        }
-	    }
+			/* otherwise go straight to return */
+			} else {
+				/* set altitude setpoint to current altitude */
+				_rtl_state = RTL_STATE_RETURN;
+				_mission_item.altitude_is_relative = false;
+				_mission_item.altitude = _navigator->get_global_position()->alt;
+			}
+		}
 
 		set_rtl_item(pos_sp_triplet);
 
@@ -132,15 +132,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-    if (_rtl_state == RTL_STATE_FINISHED) {
-        /* RTL finished, nothing to do */
-        pos_sp_triplet->next.valid = false;
-        mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
-        return;
-    }
+	if (_rtl_state == RTL_STATE_FINISHED) {
+		/* RTL finished, nothing to do */
+		pos_sp_triplet->next.valid = false;
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
+		return;
+	}
 
-    set_previous_pos_setpoint(pos_sp_triplet);
-    _navigator->set_can_loiter_at_sp(false);
+	set_previous_pos_setpoint(pos_sp_triplet);
+	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -241,11 +241,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-    /* convert mission item to current position setpoint and make it valid */
-    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-    reset_mission_item_reached();
-    pos_sp_triplet->current.valid = true;
-    pos_sp_triplet->next.valid = false;
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	reset_mission_item_reached();
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
 }
 
 void

From 9ae44291b169a90945f090487e3757db9fc8c075 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 00:27:08 +0200
Subject: [PATCH 50/61] navigator: added NAV_CMD_IDLE, added RTL_STATE_LOITER
 and RTL_STATE_LANDED instead of FINISHED

---
 src/modules/navigator/mission_block.cpp | 25 +++++---
 src/modules/navigator/rtl.cpp           | 84 ++++++++++++++++++-------
 src/modules/navigator/rtl.h             |  3 +-
 src/modules/uORB/topics/mission.h       |  1 +
 4 files changed, 82 insertions(+), 31 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 41d81ad9b0..5e545706d1 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -69,10 +69,10 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
-	/* don't check landed WPs */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return false;
+		return _navigator_priv->get_vstatus()->condition_landed;
 	}
+
 	/* TODO: count turns */
 #if 0
 	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
@@ -178,19 +178,28 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->loiter_direction = item->loiter_direction;
 	sp->pitch_min = item->pitch_min;
 
-	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+	switch (item->nav_cmd) {
+	case NAV_CMD_IDLE:
+		sp->type = SETPOINT_TYPE_IDLE;
+		break;
+
+	case NAV_CMD_TAKEOFF:
 		sp->type = SETPOINT_TYPE_TAKEOFF;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
+	case NAV_CMD_LAND:
 		sp->type = SETPOINT_TYPE_LAND;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+	case NAV_CMD_LOITER_TIME_LIMIT:
+	case NAV_CMD_LOITER_TURN_COUNT:
+	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = SETPOINT_TYPE_LOITER;
+		break;
 
-	} else {
+	default:
 		sp->type = SETPOINT_TYPE_POSITION;
+		break;
 	}
 }
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c5bb06c3b7..b7961a260c 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -53,6 +53,8 @@
 #include "navigator.h"
 #include "rtl.h"
 
+#define DELAY_SIGMA	0.01f
+
 RTL::RTL(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
 	MissionBlock(navigator),
@@ -95,7 +97,7 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		if (_rtl_state == RTL_STATE_NONE) {
 			/* for safety reasons don't go into RTL if landed */
 			if (_navigator->get_vstatus()->condition_landed) {
-				_rtl_state = RTL_STATE_FINISHED;
+				_rtl_state = RTL_STATE_LANDED;
 				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
 
 			/* if lower than return altitude, climb up first */
@@ -113,11 +115,9 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		}
 
 		set_rtl_item(pos_sp_triplet);
-
 		updated = true;
-	}
 
-	if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) {
+	} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
 		advance_rtl();
 		set_rtl_item(pos_sp_triplet);
 		updated = true;
@@ -132,19 +132,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-	if (_rtl_state == RTL_STATE_FINISHED) {
-		/* RTL finished, nothing to do */
-		pos_sp_triplet->next.valid = false;
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
-		return;
-	}
-
 	set_previous_pos_setpoint(pos_sp_triplet);
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
-
 		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
 
 		_mission_item.lat = _navigator->get_global_position()->lat;
@@ -165,8 +157,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_RETURN: {
 
+	case RTL_STATE_RETURN: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		 // don't change altitude
@@ -196,8 +188,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_DESCEND: {
 
+	case RTL_STATE_DESCEND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -207,9 +199,9 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 		_mission_item.acceptance_radius = _param_acceptance_radius.get();
-		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
+		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
@@ -217,8 +209,35 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-	case RTL_STATE_LAND: {
+	case RTL_STATE_LOITER: {
+		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
 
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = autoland;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+
+		if (autoland) {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+		}
+		break;
+	}
+
+	case RTL_STATE_LAND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -237,6 +256,25 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
+	case RTL_STATE_LANDED: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_IDLE;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+		break;
+	}
+
 	default:
 		break;
 	}
@@ -262,18 +300,20 @@ RTL::advance_rtl()
 
 	case RTL_STATE_DESCEND:
 		/* only go to land if autoland is enabled */
-		if (_param_land_delay.get() < 0) {
-			_rtl_state = RTL_STATE_FINISHED;
+		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
+			_rtl_state = RTL_STATE_LOITER;
+
 		} else {
 			_rtl_state = RTL_STATE_LAND;
 		}
 		break;
 
-	case RTL_STATE_LAND:
-		_rtl_state = RTL_STATE_FINISHED;
+	case RTL_STATE_LOITER:
+		_rtl_state = RTL_STATE_LAND;
 		break;
 
-	case RTL_STATE_FINISHED:
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_LANDED;
 		break;
 
 	default:
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index bcccf74548..c4286b391f 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -97,8 +97,9 @@ private:
 		RTL_STATE_CLIMB,
 		RTL_STATE_RETURN,
 		RTL_STATE_DESCEND,
+		RTL_STATE_LOITER,
 		RTL_STATE_LAND,
-		RTL_STATE_FINISHED,
+		RTL_STATE_LANDED,
 	} _rtl_state;
 
 	control::BlockParamFloat _param_return_alt;
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index d25db3a772..d9dd61df12 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -50,6 +50,7 @@
 
 /* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
+	NAV_CMD_IDLE=0,
 	NAV_CMD_WAYPOINT=16,
 	NAV_CMD_LOITER_UNLIMITED=17,
 	NAV_CMD_LOITER_TURN_COUNT=18,

From 1bae18377a444279f91b75281ffde5da70cc6086 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 00:29:00 +0200
Subject: [PATCH 51/61] navigator: is_mission_item_reached() for LOITER items
 fixed

---
 src/modules/navigator/mission_block.cpp | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 5e545706d1..8ef7be4496 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -74,15 +74,10 @@ MissionBlock::is_mission_item_reached()
 	}
 
 	/* TODO: count turns */
-#if 0
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
+	if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
 		return false;
 	}
-#endif
 
 	hrt_abstime now = hrt_absolute_time();
 

From 52eb49ba0bd1ea5a05845350f1b3c46f0b059a39 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 11:09:49 +0200
Subject: [PATCH 52/61] navigator: use common "acceptance radius" parameter for
 all modes

---
 src/modules/navigator/mission_block.cpp  |  4 ++--
 src/modules/navigator/navigator.h        |  4 ++--
 src/modules/navigator/navigator_main.cpp |  2 +-
 src/modules/navigator/navigator_params.c |  6 +++---
 src/modules/navigator/rtl.cpp            | 15 +++++++--------
 src/modules/navigator/rtl.h              |  1 -
 src/modules/navigator/rtl_params.c       | 12 ------------
 7 files changed, 15 insertions(+), 29 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 8ef7be4496..4f177d76ea 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -100,12 +100,12 @@ MissionBlock::is_mission_item_reached()
 		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter */
 			if (_navigator_priv->get_global_position()->alt >
-				altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
+				altitude_amsl - _navigator_priv->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 4709f71965..184ecd365f 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -115,7 +115,7 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
-	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
+	float		get_acceptance_radius() { return _param_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
@@ -165,7 +165,7 @@ private:
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
-	control::BlockParamFloat _param_takeoff_acceptance_radius;	/**< acceptance for takeoff */
+	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
 	/**
 	 * Retrieve global position
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e22c4d72df..546602741f 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -123,7 +123,7 @@ Navigator::Navigator() :
 	_rtl(this, "RTL"),
 	_update_triplet(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
-	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
+	_param_acceptance_radius(this, "ACC_RAD")
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index ca31adecc6..084afe3401 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -55,12 +55,12 @@
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
 /**
- * Takeoff Acceptance Radius (FW only)
+ * Acceptance Radius
  *
- * Acceptance radius for fixedwing.
+ * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
  * @min 1.0
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index b7961a260c..af2ff308ce 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -61,8 +61,7 @@ RTL::RTL(Navigator *navigator, const char *name) :
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY"),
-	_param_acceptance_radius(this, "ACCEPT_RAD")
+	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */
 	updateParams();
@@ -147,7 +146,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -178,7 +177,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -198,7 +197,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = false;
@@ -220,7 +219,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = autoland;
@@ -246,7 +245,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LAND;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -265,7 +264,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_IDLE;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index c4286b391f..90299c3fad 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -105,7 +105,6 @@ private:
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
-	control::BlockParamFloat _param_acceptance_radius;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 63724f4612..bfe6ce7e18 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
-
-/**
- * RTL acceptance radius
- *
- * Acceptance radius for waypoints set for RTL
- *
- * @unit meters
- * @min 1
- * @max
- * @group RTL
- */
-PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);

From affc312411b7634fa13bab6da8889de90f964ce8 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 11:34:19 +0200
Subject: [PATCH 53/61] navigator: make MissionBlock subclass of NavigatorMode

---
 src/modules/navigator/loiter.cpp        |  3 +-
 src/modules/navigator/loiter.h          |  2 +-
 src/modules/navigator/mission.cpp       |  3 +-
 src/modules/navigator/mission.h         |  2 +-
 src/modules/navigator/mission_block.cpp | 44 ++++++++++++-------------
 src/modules/navigator/mission_block.h   | 11 +++----
 src/modules/navigator/rtl.cpp           |  3 +-
 src/modules/navigator/rtl.h             |  2 +-
 8 files changed, 32 insertions(+), 38 deletions(-)

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index f5eb1e7a5a..542483fb15 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -53,8 +53,7 @@
 #include "loiter.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator)
+	MissionBlock(navigator, name)
 {
 	/* load initial params */
 	updateParams();
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 685ae61410..65ff5c31ee 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -47,7 +47,7 @@
 #include "navigator_mode.h"
 #include "mission_block.h"
 
-class Loiter : public NavigatorMode, MissionBlock
+class Loiter : public MissionBlock
 {
 public:
 	/**
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index c09f617446..72255103bd 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -58,8 +58,7 @@
 #include "mission.h"
 
 Mission::Mission(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator),
+	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 842c8c9ee8..6e47619467 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -62,7 +62,7 @@
 
 class Navigator;
 
-class Mission : public NavigatorMode, MissionBlock
+class Mission : public MissionBlock
 {
 public:
 	/**
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 4f177d76ea..9b8d3d9c70 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -52,13 +52,13 @@
 #include "mission_block.h"
 
 
-MissionBlock::MissionBlock(Navigator *navigator) :
+MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
 	_mission_item({0}),
-	_mission_item_valid(false),
-	_navigator_priv(navigator)
+	_mission_item_valid(false)
 {
 }
 
@@ -70,7 +70,7 @@ bool
 MissionBlock::is_mission_item_reached()
 {
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return _navigator_priv->get_vstatus()->condition_landed;
+		return _navigator->get_vstatus()->condition_landed;
 	}
 
 	/* TODO: count turns */
@@ -88,24 +88,24 @@ MissionBlock::is_mission_item_reached()
 		float dist_z = -1.0f;
 
 		float altitude_amsl = _mission_item.altitude_is_relative
-				      ? _mission_item.altitude + _navigator_priv->get_home_position()->alt
+				      ? _mission_item.altitude + _navigator->get_home_position()->alt
 			              : _mission_item.altitude;
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
-				                          _navigator_priv->get_global_position()->lat,
-							  _navigator_priv->get_global_position()->lon,
-							  _navigator_priv->get_global_position()->alt,
+				                          _navigator->get_global_position()->lat,
+							  _navigator->get_global_position()->lon,
+							  _navigator->get_global_position()->alt,
 				&dist_xy, &dist_z);
 
-		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter */
-			if (_navigator_priv->get_global_position()->alt >
-				altitude_amsl - _navigator_priv->get_acceptance_radius()) {
+			if (_navigator->get_global_position()->alt >
+				altitude_amsl - _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
@@ -119,10 +119,10 @@ MissionBlock::is_mission_item_reached()
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
-		if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
 
 			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
 
 			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
 				_waypoint_yaw_reached = true;
@@ -167,7 +167,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->valid = true;
 	sp->lat = item->lat;
 	sp->lon = item->lon;
-	sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
 	sp->yaw = item->yaw;
 	sp->loiter_radius = item->loiter_radius;
 	sp->loiter_direction = item->loiter_direction;
@@ -211,25 +211,25 @@ bool
 MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
     /* don't change setpoint if 'can_loiter_at_sp' flag set */
-	if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
+	if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
 		/* use current position */
-		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
+		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
 		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
 
-	    _navigator_priv->set_can_loiter_at_sp(true);
+	    _navigator->set_can_loiter_at_sp(true);
 	}
 
     if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
-            || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
+            || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
             || pos_sp_triplet->current.loiter_direction != 1
             || pos_sp_triplet->previous.valid
             || !pos_sp_triplet->current.valid
             || pos_sp_triplet->next.valid) {
         /* position setpoint triplet should be updated */
         pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-        pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+        pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
         pos_sp_triplet->current.loiter_direction = 1;
 
         pos_sp_triplet->previous.valid = false;
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 8726964fa3..f990027522 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -47,17 +47,17 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 
+#include "navigator_mode.h"
+
 class Navigator;
 
-class MissionBlock
+class MissionBlock : public NavigatorMode
 {
 public:
 	/**
 	 * Constructor
-	 *
-	 * @param pointer to parent class
 	 */
-	MissionBlock(Navigator *navigator);
+	MissionBlock(Navigator *navigator, const char *name);
 
 	/**
 	 * Destructor
@@ -101,9 +101,6 @@ public:
 
 	mission_item_s _mission_item;
 	bool _mission_item_valid;
-
-private:
-	Navigator *_navigator_priv;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index af2ff308ce..043f773a43 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -56,8 +56,7 @@
 #define DELAY_SIGMA	0.01f
 
 RTL::RTL(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator),
+	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 90299c3fad..b4b729e898 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -54,7 +54,7 @@
 
 class Navigator;
 
-class RTL : public NavigatorMode, MissionBlock
+class RTL : public MissionBlock
 {
 public:
 	/**

From d45cc69d1d62a2ccac00757ab02a731fca6d6ece Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 27 Jun 2014 21:44:21 +0200
Subject: [PATCH 54/61] mtecs/wind: store wind variance

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 93ed18b8d9..272ad53440 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1345,8 +1345,8 @@ FixedwingEstimator::task_main()
 					_wind.timestamp = _global_pos.timestamp;
 					_wind.windspeed_north = _ekf->states[14];
 					_wind.windspeed_east = _ekf->states[15];
-					_wind.covariance_north = 0.0f; // XXX get form filter
-					_wind.covariance_east = 0.0f;
+					_wind.covariance_north = _ekf->P[14][14];
+					_wind.covariance_east = _ekf->P[15][15];
 
 					/* lazily publish the wind estimate only once available */
 					if (_wind_pub > 0) {

From a398d5cbcc1d39d83611543b787c5c8aecae10dd Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Fri, 27 Jun 2014 20:33:39 -0700
Subject: [PATCH 55/61] Support for List, Open, Read, Terminate commands

Fixed various bugs. Flattened Mavlink::Session class while fixing bugs
in this area.
---
 src/modules/mavlink/mavlink_ftp.cpp  | 169 ++++++++++++---------------
 src/modules/mavlink/mavlink_ftp.h    |  33 ++----
 src/modules/mavlink/mavlink_main.cpp |  50 ++++----
 src/modules/mavlink/mavlink_main.h   |   3 +
 4 files changed, 121 insertions(+), 134 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index bfee17342a..f1436efb87 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -55,6 +55,11 @@ MavlinkFTP::MavlinkFTP()
 	// initialise the request freelist
 	dq_init(&_workFree);
 	sem_init(&_lock, 0, 1);
+    
+    // initialize session list
+    for (size_t i=0; i<kMaxSession; i++) {
+        _session_fds[i] = -1;
+    }
 
 	// drop work entries onto the free list
 	for (unsigned i = 0; i < kRequestQueueSize; i++) {
@@ -122,13 +127,11 @@ MavlinkFTP::_worker(Request *req)
 		break;
 
 	case kCmdTerminate:
-		if (!Session::terminate(hdr->session)) {
-			errorCode = kErrNoRequest;
-		}
+        errorCode = _workTerminate(req);
 		break;
 
 	case kCmdReset:
-		Session::reset();
+		errorCode = _workReset();
 		break;
 
 	case kCmdList:
@@ -219,6 +222,12 @@ MavlinkFTP::_workList(Request *req)
 
 		// no more entries?
 		if (result == nullptr) {
+            if (hdr->offset != 0 && offset == 0) {
+                // User is requesting subsequent dir entries but there were none. This means the user asked
+                // to seek past EOF.
+                errorCode = kErrEOF;
+            }
+            // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
 			break;
 		}
 
@@ -256,20 +265,21 @@ MavlinkFTP::ErrorCode
 MavlinkFTP::_workOpen(Request *req, bool create)
 {
 	auto hdr = req->header();
-
-	// allocate session ID
-	int session = Session::allocate();
-	if (session < 0) {
+    
+    int session_index = _findUnusedSession();
+	if (session_index < 0) {
 		return kErrNoSession;
 	}
 
-	// get the session to open the file
-	if (!Session::get(session)->open(req->dataAsCString(), create)) {
+	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
+    
+    int fd = ::open(req->dataAsCString(), oflag);
+	if (fd < 0) {
 		return create ? kErrPerm : kErrNotFile;
 	}
+    _session_fds[session_index] = fd;
 
-	// save the session ID in the reply
-	hdr->session = session;
+	hdr->session = session_index;
 	hdr->size = 0;
 
 	return kErrNone;
@@ -280,29 +290,40 @@ MavlinkFTP::_workRead(Request *req)
 {
 	auto hdr = req->header();
 
-	// look up session
-	auto session = Session::get(hdr->session);
-	if (session == nullptr) {
+    int session_index = hdr->session;
+    
+    if (!_validSession(session_index)) {
 		return kErrNoSession;
+    }
+    
+    // Seek to the specified position
+    printf("Seek %d\n", hdr->offset);
+	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
+        // Unable to see to the specified location
+		return kErrEOF;
 	}
-
-	// read from file
-	int result = session->read(hdr->offset, &hdr->data[0], hdr->size);
-
-	if (result < 0) {
+    
+	int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
+	if (bytes_read < 0) {
+        // Negative return indicates error other than eof
 		return kErrIO;
 	}
-	hdr->size = result;
+    
+    printf("Read success %d\n", bytes_read);
+	hdr->size = bytes_read;
+    
 	return kErrNone;
 }
 
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workWrite(Request *req)
 {
+#if 0
+    // NYI: Coming soon
 	auto hdr = req->header();
 
 	// look up session
-	auto session = Session::get(hdr->session);
+	auto session = getSession(hdr->session);
 	if (session == nullptr) {
 		return kErrNoSession;
 	}
@@ -317,6 +338,9 @@ MavlinkFTP::_workWrite(Request *req)
 
 	hdr->size = result;
 	return kErrNone;
+#else
+	return kErrPerm;
+#endif
 }
 
 MavlinkFTP::ErrorCode
@@ -328,91 +352,52 @@ MavlinkFTP::_workRemove(Request *req)
 	return kErrPerm;
 }
 
-MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession];
-
-int
-MavlinkFTP::Session::allocate()
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTerminate(Request *req)
 {
-	for (unsigned i = 0; i < kMaxSession; i++) {
-		if (_sessions[i]._fd < 0) {
-			return i;
-		}
-	}
-	return -1;
+	auto hdr = req->header();
+    
+    if (!_validSession(hdr->session)) {
+		return kErrNoSession;
+    }
+    
+    ::close(_session_fds[hdr->session]);
+
+    return kErrNone;
 }
 
-MavlinkFTP::Session *
-MavlinkFTP::Session::get(unsigned index)
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workReset(void)
 {
-	if ((index >= kMaxSession) || (_sessions[index]._fd < 0)) {
-		return nullptr;
-	}
-	return &_sessions[index];
-}
-
-void
-MavlinkFTP::Session::terminate()
-{
-	// clean up aborted transfers?
-	if (_fd >= 0) {
-		close(_fd);
-		_fd = -1;
-	}
+    for (size_t i=0; i<kMaxSession; i++) {
+        if (_session_fds[i] != -1) {
+            ::close(_session_fds[i]);
+            _session_fds[i] = -1;
+        }
+    }
+    
+    return kErrNone;
 }
 
 bool
-MavlinkFTP::Session::terminate(unsigned index)
- {
-	Session *session = get(index);
-
-	if (session == nullptr) {
-		return false;
-	}
-
-	session->terminate();
-	return true;
-}
-
-void
-MavlinkFTP::Session::reset()
+MavlinkFTP::_validSession(unsigned index)
 {
-	for (unsigned i = 0; i < kMaxSession; i++) {
-		terminate(i);
-	}
-}
-
-bool
-MavlinkFTP::Session::open(const char *path, bool create)
-{
-	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
-
-	_fd = open(path, oflag);
-	if (_fd < 0) {
+	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
 		return false;
 	}
 	return true;
 }
 
 int
-MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count)
+MavlinkFTP::_findUnusedSession(void)
 {
-	// can we seek to the location?
-	if (lseek(_fd, offset, SEEK_SET) < 0) {
-		return -1;
-	}
-
-	return read(_fd, buf, count);
-}
-
-int
-MavlinkFTP::Session::append(off_t offset, uint8_t *buf, uint8_t count)
-{
-	// make sure that the requested offset matches our current position
-	off_t pos = lseek(_fd, 0, SEEK_CUR);
-	if (pos != offset) {
-		return -1;
-	}
-	return write(_fd, buf, count);
+    for (size_t i=0; i<kMaxSession; i++) {
+        if (_session_fds[i] == -1) {
+            return i;
+        }
+    }
+    
+    return -1;
 }
 
 char *
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 462594301c..8a1b3fbaae 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -115,27 +115,11 @@ private:
 		kErrPerm
 	};
 
-	class Session
-	{
-	public:
-		Session() : _fd(-1) {}
+    int _findUnusedSession(void);
+    bool _validSession(unsigned index);
 
-		static int	allocate();
-		static Session	*get(unsigned index);
-		static bool	terminate(unsigned index);
-		static void	reset();
-
-		void		terminate();
-		bool		open(const char *path, bool create);
-		int		read(off_t offset, uint8_t *buf, uint8_t count);
-		int		append(off_t offset, uint8_t *buf, uint8_t count);
-
-	private:
-		static const unsigned kMaxSession = 2;
-		static Session	_sessions[kMaxSession];
-
-		int		_fd;
-	};
+    static const unsigned kMaxSession = 2;
+    int	_session_fds[kMaxSession];
 
 	class Request
 	{
@@ -163,7 +147,11 @@ private:
 			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
 				_mavlink->get_channel(), &msg, sequence(), rawData());
 
-			if (!_mavlink->message_buffer_write(&msg, len)) {
+            _mavlink->lockMessageBufferMutex();
+            bool fError = _mavlink->message_buffer_write(&msg, len);
+            _mavlink->unlockMessageBufferMutex();
+            
+            if (!fError) {
 				warnx("FTP TX ERR");
 			} else {
 				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
@@ -211,6 +199,8 @@ private:
 	ErrorCode		_workRead(Request *req);
 	ErrorCode		_workWrite(Request *req);
 	ErrorCode		_workRemove(Request *req);
+    ErrorCode       _workTerminate(Request *req);
+    ErrorCode       _workReset();
 
 	// work freelist
 	Request			_workBufs[kRequestQueueSize];
@@ -232,4 +222,5 @@ private:
 		_qUnlock();
 		return req;
 	}
+    
 };
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e9cce175d9..c77f3c6ca1 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2074,38 +2074,46 @@ Mavlink::task_main(int argc, char *argv[])
 		if (_passing_on || _ftp_on) {
 
 			bool is_part;
-			void *read_ptr;
+			uint8_t *read_ptr;
+            uint8_t *write_ptr;
 
-			/* guard get ptr by mutex */
 			pthread_mutex_lock(&_message_buffer_mutex);
-			int available = message_buffer_get_ptr(&read_ptr, &is_part);
+			int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
 			pthread_mutex_unlock(&_message_buffer_mutex);
 
 			if (available > 0) {
+                // Reconstruct message from buffer
 
-				// int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq;
-				
-				const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr;
-				/* write first part of buffer */
-				_mavlink_resend_uart(_channel, msg);
-
-				// mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq;
-				// mavlink_msg_system_time_send(get_channel(), 255, 255);
-
-				message_buffer_mark_read(available);
+				mavlink_message_t msg;
+                write_ptr = (uint8_t*)&msg;
 
+                // Pull a single message from the buffer
+                int read_count = available;
+                if (read_count > sizeof(mavlink_message_t)) {
+                    read_count = sizeof(mavlink_message_t);
+                }
+                
+                memcpy(write_ptr, read_ptr, read_count);
+                
+                // We hold the mutex until after we complete the second part of the buffer. If we don't
+                // we may end up breaking the empty slot overflow detection semantics when we mark the
+                // possibly partial read below.
+                pthread_mutex_lock(&_message_buffer_mutex);
+                
+				message_buffer_mark_read(read_count);
 
 				/* write second part of buffer if there is some */
-				// XXX this doesn't quite work, as the resend UART call assumes a continous block
-				if (is_part) {
-					/* guard get ptr by mutex */
-					pthread_mutex_lock(&_message_buffer_mutex);
-					available = message_buffer_get_ptr(&read_ptr, &is_part);
-					pthread_mutex_unlock(&_message_buffer_mutex);
-
-					_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+				if (is_part && read_count < sizeof(mavlink_message_t)) {
+                    write_ptr += read_count;
+					available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+                    read_count = sizeof(mavlink_message_t) - read_count;
+                    memcpy(write_ptr, read_ptr, read_count);
 					message_buffer_mark_read(available);
 				}
+                
+                pthread_mutex_unlock(&_message_buffer_mutex);
+
+                _mavlink_resend_uart(_channel, &msg);
 			}
 		}
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 76ce429432..d44db08194 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -228,6 +228,9 @@ public:
 	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
 
 	bool			message_buffer_write(void *ptr, int size);
+    
+    void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+    void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
 
 protected:
 	Mavlink			*next;

From 503ded05394767d58359834e73bc63672b701dbe Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 Jun 2014 12:35:04 +0200
Subject: [PATCH 56/61] Remove old TECS implementation - we can really only
 decently flight-test and support one.

---
 makefiles/config_aerocore_default.mk          |  1 -
 makefiles/config_px4fmu-v1_default.mk         |  1 -
 makefiles/config_px4fmu-v2_default.mk         |  1 -
 .../fw_pos_control_l1_main.cpp                | 74 +++++--------------
 4 files changed, 17 insertions(+), 60 deletions(-)

diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk
index d6902c4ee2..fd112b32d5 100644
--- a/makefiles/config_aerocore_default.mk
+++ b/makefiles/config_aerocore_default.mk
@@ -80,7 +80,6 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index e4bc6fecf1..f943f62f2f 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -107,7 +107,6 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index a68cc32f47..83dd85adb9 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -120,7 +120,6 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 5df17e2ecf..f2c5db8068 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -43,8 +43,8 @@
  *    Proceedings of the AIAA Guidance, Navigation and Control
  *    Conference, Aug 2004. AIAA-2004-4900.
  *
- * Original implementation for total energy control class:
- *    Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
+ * Implementation for total energy control class:
+ *    Thomas Gubler
  *
  * More details and acknowledgements in the referenced library headers.
  *
@@ -88,7 +88,6 @@
 #include <mavlink/mavlink_log.h>
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
-#include <external_lgpl/tecs/tecs.h>
 #include <drivers/drv_range_finder.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
@@ -201,7 +200,6 @@ private:
 	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
-	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
 	bool						_was_pos_control_mode;
 
@@ -568,23 +566,6 @@ FixedwingPositionControl::parameters_update()
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
 
-	_tecs.set_time_const(_parameters.time_const);
-	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
-	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
-	_tecs.set_throttle_damp(_parameters.throttle_damp);
-	_tecs.set_integrator_gain(_parameters.integrator_gain);
-	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
-	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
-	_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
-	_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
-	_tecs.set_speed_weight(_parameters.speed_weight);
-	_tecs.set_pitch_damping(_parameters.pitch_damping);
-	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
-	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
-	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
-	_tecs.set_heightrate_p(_parameters.heightrate_p);
-	_tecs.set_speedrate_p(_parameters.speedrate_p);
-
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||
 	    _parameters.airspeed_max < 5.0f ||
@@ -656,9 +637,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 		}
 	}
 
-	/* update TECS state */
-	_tecs.enable_airspeed(_airspeed_valid);
-
 	return airspeed_updated;
 }
 
@@ -839,10 +817,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
-	if (!_mTecs.getEnabled()) {
-		_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
-	}
-
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
@@ -867,9 +841,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
 
-		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
-		_tecs.set_speed_weight(_parameters.speed_weight);
-
 		/* current waypoint (the one currently heading for) */
 		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
@@ -1222,8 +1193,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* user switched off throttle */
 		if (_manual.z < 0.1f) {
 			throttle_max = 0.0f;
-			/* switch to pure pitch based altitude control, give up speed */
-			_tecs.set_speed_weight(0.0f);
 		}
 
 		/* climb out control */
@@ -1263,9 +1232,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	}
 	else {
-		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max);
+		_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
 	}
-	_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
+	_att_sp.pitch_body = _mTecs.getPitchSetpoint();
 
 	if (_control_mode.flag_control_position_enabled) {
 		last_manual = false;
@@ -1449,29 +1418,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		const math::Vector<3> &ground_speed,
 		tecs_mode mode)
 {
-	if (_mTecs.getEnabled()) {
-		/* Using mtecs library: prepare arguments for mtecs call */
-		float flightPathAngle = 0.0f;
-		float ground_speed_length = ground_speed.length();
-		if (ground_speed_length > FLT_EPSILON) {
-			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
-		}
-		fwPosctrl::LimitOverride limitOverride;
-		if (climbout_mode) {
-			limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
-		} else {
-			limitOverride.disablePitchMinOverride();
-		}
-		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
-				limitOverride);
-	} else {
-		/* Using tecs library */
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
-					    _airspeed.indicated_airspeed_m_s, eas2tas,
-					    climbout_mode, climbout_pitch_min_rad,
-					    throttle_min, throttle_max, throttle_cruise,
-					    pitch_min_rad, pitch_max_rad);
+	/* Using mtecs library: prepare arguments for mtecs call */
+	float flightPathAngle = 0.0f;
+	float ground_speed_length = ground_speed.length();
+	if (ground_speed_length > FLT_EPSILON) {
+		flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
 	}
+	fwPosctrl::LimitOverride limitOverride;
+	if (climbout_mode) {
+		limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+	} else {
+		limitOverride.disablePitchMinOverride();
+	}
+	_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+			limitOverride);
 }
 
 int

From 2bd5511d096c3351bf74c81072e2cc00b344ef79 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 Jun 2014 12:37:26 +0200
Subject: [PATCH 57/61] geo lookup lib: Moved to separate module and compiling
 with -Os to save some precious flash

---
 src/lib/geo/geo.h                             |  2 +-
 src/lib/geo/module.mk                         |  5 +--
 .../{geo => geo_lookup}/geo_mag_declination.c |  0
 .../{geo => geo_lookup}/geo_mag_declination.h |  0
 src/lib/geo_lookup/module.mk                  | 40 +++++++++++++++++++
 5 files changed, 43 insertions(+), 4 deletions(-)
 rename src/lib/{geo => geo_lookup}/geo_mag_declination.c (100%)
 rename src/lib/{geo => geo_lookup}/geo_mag_declination.h (100%)
 create mode 100644 src/lib/geo_lookup/module.mk

diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 87c1cf4605..8b286af36a 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -50,7 +50,7 @@
 
 __BEGIN_DECLS
 
-#include "geo_mag_declination.h"
+#include "geo_lookup/geo_mag_declination.h"
 
 #include <stdbool.h>
 
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
index 9500a2bccf..d08ca4532f 100644
--- a/src/lib/geo/module.mk
+++ b/src/lib/geo/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -35,5 +35,4 @@
 # Geo library
 #
 
-SRCS		=	geo.c \
-			geo_mag_declination.c
+SRCS		=	geo.c
diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c
similarity index 100%
rename from src/lib/geo/geo_mag_declination.c
rename to src/lib/geo_lookup/geo_mag_declination.c
diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h
similarity index 100%
rename from src/lib/geo/geo_mag_declination.h
rename to src/lib/geo_lookup/geo_mag_declination.h
diff --git a/src/lib/geo_lookup/module.mk b/src/lib/geo_lookup/module.mk
new file mode 100644
index 0000000000..d7a10df2db
--- /dev/null
+++ b/src/lib/geo_lookup/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Geo lookup table / data library
+#
+
+SRCS		 =	geo_mag_declination.c
+
+MAXOPTIMIZATION	 = -Os

From 13f9ce9ddd0f6ab4b8fd4f7be3234e6989da8ea9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 Jun 2014 12:38:31 +0200
Subject: [PATCH 58/61] Comment fix in perf counter header, no code changes.

---
 src/modules/systemlib/perf_counter.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 6835ee4a2f..668d9dfdf6 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -75,7 +75,7 @@ __EXPORT extern void		perf_free(perf_counter_t handle);
 /**
  * Count a performance event.
  *
- * This call only affects counters that take single events; PC_COUNT etc.
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
  *
  * @param handle		The handle returned from perf_alloc.
  */

From e78d496e921e9c50a3efc48a0a5875be1616c788 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 Jun 2014 12:51:22 +0200
Subject: [PATCH 59/61] Enable new lookup lib

---
 makefiles/config_aerocore_default.mk  | 1 +
 makefiles/config_px4fmu-v1_default.mk | 1 +
 makefiles/config_px4fmu-v2_default.mk | 1 +
 3 files changed, 3 insertions(+)

diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk
index d6902c4ee2..0d3934bd01 100644
--- a/makefiles/config_aerocore_default.mk
+++ b/makefiles/config_aerocore_default.mk
@@ -82,6 +82,7 @@ MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
 MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 810e008aeb..9c7f326323 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -113,6 +113,7 @@ MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
 MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index a68cc32f47..29fb9ebac0 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -122,6 +122,7 @@ MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
 MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 

From 8d1ed164cb6efc533da9ef8ba6c94a00ef329d30 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 29 Jun 2014 12:51:40 +0200
Subject: [PATCH 60/61] fw att ctrl: resolve warnings

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 3 ---
 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp  | 3 ---
 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp   | 7 +------
 3 files changed, 1 insertion(+), 12 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index a0a18bc2e4..46db788a6b 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0;
-
 	/* input conditioning */
 	if (!isfinite(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index d2a231694d..9894a34d75 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0; //xxx: param
-
 	/* input conditioning */
 //	warnx("airspeed pre %.4f", (double)airspeed);
 	if (!isfinite(airspeed)) {
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 79184e2cdd..fe03b80651 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch,
 	_rate_setpoint = 0.0f;
 	if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
 		float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
-		if(denumerator != 0.0f) { //XXX: floating point comparison
+		if(fabsf(denumerator) > FLT_EPSILON) {
 			_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
 //			warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
 		}
@@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0;
-
-
 	/* input conditioning */
 	if (!isfinite(airspeed)) {
 	/* airspeed is NaN, +- INF or not available, pick center of band */

From 263a2fa9b2258c02bdb0df6746bc289338d63cca Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 29 Jun 2014 14:52:09 +0200
Subject: [PATCH 61/61] mtecs: add flightpathangle filter

---
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 19 +++++++----
 src/modules/fw_pos_control_l1/mtecs/mTecs.h   | 34 +++++++++++--------
 .../fw_pos_control_l1/mtecs/mTecs_params.c    |  7 ++++
 src/modules/sdlog2/sdlog2.c                   |  1 +
 src/modules/sdlog2/sdlog2_messages.h          |  3 +-
 src/modules/uORB/topics/tecs_status.h         |  1 +
 6 files changed, 43 insertions(+), 22 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 48c9079a48..fc0a2551c8 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -58,6 +58,7 @@ mTecs::mTecs() :
 	_controlEnergyDistribution(this, "PIT", true),
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
+	_flightPathAngleLowpass(this, "FPA_LP"),
 	_airspeedLowpass(this, "A_LP"),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
@@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	/* time measurement */
 	updateTimeMeasurement();
 
-	/* Filter arispeed */
+	/* Filter airspeed */
 	float airspeedFiltered = _airspeedLowpass.update(airspeed);
 
 	/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
@@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	}
 
 	/* Write part of the status message */
-	_status.flightPathAngleSp = flightPathAngleSp;
-	_status.flightPathAngle = flightPathAngle;
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed = airspeed;
 	_status.airspeedFiltered = airspeedFiltered;
@@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* update parameters first */
 	updateParams();
 
+	/* Filter flightpathangle */
+	float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
+
 	/* calculate values (energies) */
-	float flightPathAngleError = flightPathAngleSp - flightPathAngle;
+	float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
 	float airspeedDerivative = 0.0f;
 	if(_airspeedDerivative.getDt() > 0.0f) {
 		airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
@@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
 	float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
 
-	float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm;
+	float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
 	float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
 	float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
 	float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
 
-	float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm;
+	float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
 	float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
 	float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
 	float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
@@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
 	if (mode == TECS_MODE_TAKEOFF) {
-		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
+		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
 	} else if (mode == TECS_MODE_LAND) {
 		// only limit pitch but do not limit throttle
@@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
 	/* Write part of the status message */
+	_status.flightPathAngleSp = flightPathAngleSp;
+	_status.flightPathAngle = flightPathAngle;
+	_status.flightPathAngleFiltered = flightPathAngleFiltered;
 	_status.airspeedDerivativeSp = airspeedDerivativeSp;
 	_status.airspeedDerivative = airspeedDerivative;
 	_status.totalEnergyRateSp = totalEnergyRateSp;
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index c102f5dee5..efa89a5d3d 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -104,12 +104,17 @@ protected:
 	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
 
 	/* control blocks */
-	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
-	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control: output is pitch */
-	BlockPDLimited	_controlAltitude;		/**< PD controller for altitude: output is the flight path angle setpoint */
-	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration setpoint */
+	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
+								  is throttle */
+	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control:
+								  output is pitch */
+	BlockPDLimited	_controlAltitude;			/**< PD controller for altitude: output is the flight
+								  path angle setpoint */
+	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration
+								  setpoint */
 
 	/* Other calculation Blocks */
+	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
 	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
@@ -118,21 +123,22 @@ protected:
 	float _pitchSp;					/**< Pitch Setpoint from -pi to pi */
 
 	/* Output Limits in special modes */
-	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;		/**< Throttle Limits during takeoff */
-	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;      	/**< Pitch Limit during takeoff */
-	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;	/**< Throttle Limits when underspeed is detected */
-	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;   	/**< Pitch Limit when underspeed is detected */
-	BlockOutputLimiter _BlockOutputLimiterLandThrottle;		/**< Throttle Limits during landing (only in last phase)*/
-	BlockOutputLimiter _BlockOutputLimiterLandPitch;		/**< Pitch Limit during landing */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;	    /**< Throttle Limits during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;	    /**< Pitch Limit during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;   /**< Throttle Limits when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;	    /**< Pitch Limit when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterLandThrottle;	    /**< Throttle Limits during landing (only in
+								      last phase)*/
+	BlockOutputLimiter _BlockOutputLimiterLandPitch;	    /**< Pitch Limit during landing */
 
 	/* Time measurements */
-	hrt_abstime timestampLastIteration;		/**< Saves the result of hrt_absolute_time() of the last iteration */
+	hrt_abstime timestampLastIteration;	/**< Saves the result of hrt_absolute_time() of the last iteration */
 
-	bool _firstIterationAfterReset;			/**< True during the first iteration after a reset */
-	bool _dtCalculated;				/**< True if dt has been calculated in this iteration */
+	bool _firstIterationAfterReset;		/**< True during the first iteration after a reset */
+	bool _dtCalculated;			/**< True if dt has been calculated in this iteration */
 
 	int _counter;
-	bool _debug;					///< Set true to enable debug output
+	bool _debug;				///< Set true to enable debug output
 
 
 	static void	debug_print(const char *fmt, va_list args);
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index bbbb8f9dbe..5b92387807 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
  */
 PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
 
+/**
+ * Lowpass (cutoff freq.) for the flight path angle
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
+
 /**
  * P gain for the altitude control
  * Maps the altitude error to the flight path angle setpoint
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 0813bf7b0b..ab9c1639c5 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1507,6 +1507,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
+			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
 			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
 			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 5645ebcf19..a631317df5 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -353,6 +353,7 @@ struct log_TECS_s {
 	float altitude;
 	float flightPathAngleSp;
 	float flightPathAngle;
+	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeed;
 	float airspeedFiltered;
@@ -431,7 +432,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "fffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index 05310e9062..c4d0c18749 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -68,6 +68,7 @@ struct tecs_status_s {
 	float altitude;
 	float flightPathAngleSp;
 	float flightPathAngle;
+	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeed;
 	float airspeedFiltered;