Merged upstream master into gnss_rework branch

14 files changed, 1862 insertions, 632 deletions

diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
index 1c1e097a4..fccd4d9a5 100644
--- a/src/modules/mavlink/mavlink_commands.cpp
+++ b/src/modules/mavlink/mavlink_commands.cpp
@@ -40,34 +40,31 @@
 
 #include "mavlink_commands.h"
 
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
 {
 	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
-	_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
-}
-
-MavlinkCommandsStream::~MavlinkCommandsStream()
-{
 }
 
 void
 MavlinkCommandsStream::update(const hrt_abstime t)
 {
-	if (_cmd_sub->update(t)) {
+	struct vehicle_command_s cmd;
+
+	if (_cmd_sub->update(&_cmd_time, &cmd)) {
 		/* only send commands for other systems/components */
-		if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
+		if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
 			mavlink_msg_command_long_send(_channel,
-						      _cmd->target_system,
-						      _cmd->target_component,
-						      _cmd->command,
-						      _cmd->confirmation,
-						      _cmd->param1,
-						      _cmd->param2,
-						      _cmd->param3,
-						      _cmd->param4,
-						      _cmd->param5,
-						      _cmd->param6,
-						      _cmd->param7);
+						      cmd.target_system,
+						      cmd.target_component,
+						      cmd.command,
+						      cmd.confirmation,
+						      cmd.param1,
+						      cmd.param2,
+						      cmd.param3,
+						      cmd.param4,
+						      cmd.param5,
+						      cmd.param6,
+						      cmd.param7);
 		}
 	}
 }
diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h
index 6255d65df..abdba34b9 100644
--- a/src/modules/mavlink/mavlink_commands.h
+++ b/src/modules/mavlink/mavlink_commands.h
@@ -55,10 +55,10 @@ private:
 	MavlinkOrbSubscription *_cmd_sub;
 	struct vehicle_command_s *_cmd;
 	mavlink_channel_t _channel;
+	uint64_t _cmd_time;
 
 public:
 	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
-	~MavlinkCommandsStream();
 	void update(const hrt_abstime t);
 };
 
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
new file mode 100644
index 000000000..ca846a465
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -0,0 +1,415 @@
+/****************************************************************************
+ *
+ *   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 <stdio.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);
+    
+    // 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++) {
+		_qFree(&_workBufs[i]);
+	}
+
+}
+
+void
+MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
+{
+	// get a free request
+	auto req = _dqFree();
+
+	// if we couldn't get a request slot, just drop it
+	if (req != nullptr) {
+
+		// decode the request
+		if (req->decode(mavlink, msg)) {
+
+			// and queue it for the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+		} else {
+			_qFree(req);
+		}
+	}
+}
+
+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->rawData(), req->dataSize()) != messageCRC) {
+		errorCode = kErrNoRequest;
+		goto out;
+		printf("ftp: bad crc\n");
+	}
+
+	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+
+	switch (hdr->opcode) {
+	case kCmdNone:
+		break;
+
+	case kCmdTerminate:
+		errorCode = _workTerminate(req);
+		break;
+
+	case kCmdReset:
+		errorCode = _workReset();
+		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;
+		printf("FTP: ack\n");
+	} else {
+		printf("FTP: nak %u\n", errorCode);
+		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->rawData(), req->dataSize());
+
+	// then pack and send the reply back to the request source
+	req->reply();
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workList(Request *req)
+{
+	auto hdr = req->header();
+	DIR *dp = opendir(req->dataAsCString());
+
+	if (dp == nullptr) {
+		printf("FTP: can't open path '%s'\n", req->dataAsCString());
+		return kErrNotDir;
+	}
+
+	ErrorCode errorCode = kErrNone;
+	struct dirent entry, *result = nullptr;
+	unsigned offset = 0;
+
+	// move to the requested offset
+	seekdir(dp, hdr->offset);
+
+	for (;;) {
+		// read the directory entry
+		if (readdir_r(dp, &entry, &result)) {
+			errorCode = kErrIO;
+			break;
+		}
+
+		// 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;
+		}
+
+		// 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);
+		offset += strlen(entry.d_name) + 1;
+		printf("FTP: list %s\n", entry.d_name);
+	}
+
+	closedir(dp);
+	hdr->size = offset;
+
+	return errorCode;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workOpen(Request *req, bool create)
+{
+	auto hdr = req->header();
+    
+	int session_index = _findUnusedSession();
+	if (session_index < 0) {
+		return kErrNoSession;
+	}
+
+	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;
+
+	hdr->session = session_index;
+	hdr->size = 0;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRead(Request *req)
+{
+	auto hdr = req->header();
+
+	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;
+	}
+
+	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;
+	}
+
+	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 = getSession(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;
+#else
+	return kErrPerm;
+#endif
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemove(Request *req)
+{
+	auto hdr = req->header();
+
+	// for now, send error reply
+	return kErrPerm;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTerminate(Request *req)
+{
+	auto hdr = req->header();
+    
+	if (!_validSession(hdr->session)) {
+		return kErrNoSession;
+	}
+    
+	::close(_session_fds[hdr->session]);
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workReset(void)
+{
+	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::_validSession(unsigned index)
+{
+	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
+		return false;
+	}
+	return true;
+}
+
+int
+MavlinkFTP::_findUnusedSession(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] == -1) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+char *
+MavlinkFTP::Request::dataAsCString()
+{
+	// guarantee nul termination
+	if (header()->size < kMaxDataLength) {
+		requestData()[header()->size] = '\0';
+	} else {
+		requestData()[kMaxDataLength - 1] = '\0';
+	}
+
+	// and return data
+	return (char *)&(header()->data[0]);
+}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
new file mode 100644
index 000000000..e22e61553
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   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 <systemlib/err.h>
+
+#include "mavlink_messages.h"
+
+class MavlinkFTP
+{
+public:
+	MavlinkFTP();
+
+	static MavlinkFTP	*getServer();
+
+	// static interface
+	void			handle_message(Mavlink* mavlink,
+					mavlink_message_t *msg);
+
+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[];
+	};
+
+	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
+	};
+
+    int _findUnusedSession(void);
+    bool _validSession(unsigned index);
+
+    static const unsigned kMaxSession = 2;
+    int	_session_fds[kMaxSession];
+
+	class Request
+	{
+	public:
+		union {
+			dq_entry_t	entry;
+			work_s		work;
+		};
+
+		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+				_mavlink = mavlink;
+				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+				return true;
+			}
+			return false;
+		}
+
+		void		reply() {
+
+			// 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());
+
+            _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",
+					_mavlink->get_system_id(),
+					_mavlink->get_component_id(),
+					_mavlink->get_channel(),
+					len,
+					msg.checksum);
+			}
+		}
+
+		uint8_t		*rawData() { return &_message.data[0]; }
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		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 _mavlink->get_channel(); }
+
+		char		*dataAsCString();
+
+	private:
+		Mavlink			*_mavlink;
+		mavlink_encapsulated_data_t _message;
+
+	};
+
+	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
+	/// 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);
+	ErrorCode		_workTerminate(Request *req);
+	ErrorCode		_workReset();
+
+	// 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_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e300be074..9a5e31ef4 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) {
@@ -192,13 +197,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 
 			if (buf_free < desired) {
 				/* we don't want to send anything just in half, so return */
+				instance->count_txerr();
 				return;
 			}
 		}
 
 		ssize_t ret = write(uart, ch, desired);
 		if (ret != desired) {
-			warnx("TX FAIL");
+			instance->count_txerr();
 		} else {
 			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
 		}
@@ -230,14 +236,22 @@ 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),
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
 	_message_buffer({}),
+	_param_initialized(false),
+	_param_system_id(0),
+	_param_component_id(0),
+	_param_system_type(0),
+	_param_use_hil_gps(0),
+
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
 	_wpm = &_wpm_s;
 	mission.count = 0;
@@ -290,6 +304,7 @@ Mavlink::Mavlink() :
 Mavlink::~Mavlink()
 {
 	perf_free(_loop_perf);
+	perf_free(_txerr_perf);
 
 	if (_task_running) {
 		/* task wakes up every 10ms or so at the longest */
@@ -315,6 +330,12 @@ Mavlink::~Mavlink()
 }
 
 void
+Mavlink::count_txerr()
+{
+	perf_count(_txerr_perf);
+}
+
+void
 Mavlink::set_mode(enum MAVLINK_MODE mode)
 {
 	_mode = mode;
@@ -453,7 +474,7 @@ Mavlink::get_instance_id()
 	return _instance_id;
 }
 
-mavlink_channel_t
+const mavlink_channel_t
 Mavlink::get_channel()
 {
 	return _channel;
@@ -494,48 +515,53 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 
 void Mavlink::mavlink_update_system(void)
 {
-	static bool initialized = false;
-	static param_t param_system_id;
-	static param_t param_component_id;
-	static param_t param_system_type;
-	static param_t param_use_hil_gps;
 
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-		param_use_hil_gps = param_find("MAV_USEHILGPS");
-		initialized = true;
+	if (!_param_initialized) {
+		_param_system_id = param_find("MAV_SYS_ID");
+		_param_component_id = param_find("MAV_COMP_ID");
+		_param_system_type = param_find("MAV_TYPE");
+		_param_use_hil_gps = param_find("MAV_USEHILGPS");
+		_param_initialized = true;
 	}
 
 	/* update system and component id */
 	int32_t system_id;
-	param_get(param_system_id, &system_id);
+	param_get(_param_system_id, &system_id);
 
 	if (system_id > 0 && system_id < 255) {
 		mavlink_system.sysid = system_id;
 	}
 
 	int32_t component_id;
-	param_get(param_component_id, &component_id);
+	param_get(_param_component_id, &component_id);
 
 	if (component_id > 0 && component_id < 255) {
 		mavlink_system.compid = component_id;
 	}
 
 	int32_t system_type;
-	param_get(param_system_type, &system_type);
+	param_get(_param_system_type, &system_type);
 
 	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
 		mavlink_system.type = system_type;
 	}
 
 	int32_t use_hil_gps;
-	param_get(param_use_hil_gps, &use_hil_gps);
+	param_get(_param_use_hil_gps, &use_hil_gps);
 
 	_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 */
@@ -819,7 +845,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 
 					if (param == PARAM_INVALID) {
 						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[mavlink pm] unknown: %s", name);
+						sprintf(buf, "[pm] unknown: %s", name);
 						mavlink_missionlib_send_gcs_string(buf);
 
 					} else {
@@ -898,7 +924,11 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 	case MAV_CMD_NAV_TAKEOFF:
 		mission_item->pitch_min = mavlink_mission_item->param1;
 		break;
-
+	case MAV_CMD_DO_JUMP:
+		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+		mission_item->do_jump_current_count = 0;
+		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
+		break;
 	default:
 		mission_item->acceptance_radius = mavlink_mission_item->param2;
 		mission_item->time_inside = mavlink_mission_item->param1;
@@ -914,6 +944,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 	// mission_item->index = mavlink_mission_item->seq;
 	mission_item->origin = ORIGIN_MAVLINK;
 
+	/* reset DO_JUMP count */
+	mission_item->do_jump_current_count = 0;
+
 	return OK;
 }
 
@@ -931,6 +964,11 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
 		mavlink_mission_item->param1 = mission_item->pitch_min;
 		break;
 
+	case NAV_CMD_DO_JUMP:
+		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		break;
+
 	default:
 		mavlink_mission_item->param2 = mission_item->acceptance_radius;
 		mavlink_mission_item->param1 = mission_item->time_inside;
@@ -1008,8 +1046,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
 
 	} else {
 		mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
-
-		if (_verbose) { warnx("ERROR: index out of bounds"); }
 	}
 }
 
@@ -1082,8 +1118,6 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
 
 	} else {
 		mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
-
-		if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
 	}
 }
 
@@ -1114,8 +1148,6 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
 
 		mavlink_missionlib_send_gcs_string("Operation timeout");
 
-		if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); }
-
 		_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 		_wpm->current_partner_sysid = 0;
 		_wpm->current_partner_compid = 0;
@@ -1150,8 +1182,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-
-				if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
 			}
 
 			break;
@@ -1175,15 +1205,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-
-						if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); }
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-
-					if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
-
 				}
 			}
 
@@ -1214,8 +1239,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
-
-					if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
 				}
 			}
 
@@ -1233,8 +1256,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
 
-					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); }
-
 					break;
 				}
 
@@ -1245,15 +1266,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
 
 					if (wpr.seq == 0) {
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 						_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
 
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
 
-						if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
-
 						break;
 					}
 
@@ -1261,17 +1280,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wpr.seq == _wpm->current_wp_id) {
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 					} else if (wpr.seq == _wpm->current_wp_id + 1) {
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
 
-						if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); }
-
 						break;
 					}
 
@@ -1279,8 +1296,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
 
-					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); }
-
 					break;
 				}
 
@@ -1294,7 +1309,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				} else {
 					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
 
-					if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
+					mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds");
 				}
 
 
@@ -1328,15 +1343,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					}
 
 					if (wpc.count == 0) {
-						mavlink_missionlib_send_gcs_string("COUNT 0");
-
-						if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+						mavlink_missionlib_send_gcs_string("WP COUNT 0");
 
 						break;
 					}
 
-					if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
-
 					_wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
 					_wpm->current_wp_id = 0;
 					_wpm->current_partner_sysid = msg->sysid;
@@ -1350,18 +1361,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					if (_wpm->current_wp_id == 0) {
 						mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
-
 					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-
-						if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); }
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP");
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
-
-					if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
 				}
 			}
 		}
@@ -1384,7 +1389,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wp.seq != 0) {
 						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
 						break;
 					}
 
@@ -1392,12 +1396,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wp.seq >= _wpm->current_count) {
 						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
 						break;
 					}
 
 					if (wp.seq != _wpm->current_wp_id) {
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id);
+						mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch");
 						mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 						break;
 					}
@@ -1512,11 +1515,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 void
 Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
-	uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
-
-	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
-	mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
+	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+	mavlink_send_uart_bytes(_channel, buf, len);
 }
 
 
@@ -1590,31 +1592,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;
 }
 
@@ -1649,11 +1656,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 +1798,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 +1854,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 +1923,12 @@ 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) {
-		/* initialize message buffer if multiplexing is on */
-		if (OK != message_buffer_init(500)) {
+	if (_passing_on || _ftp_on) {
+		/* 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");
 		}
 
@@ -1934,9 +1958,12 @@ Mavlink::task_main(int argc, char *argv[])
 	_task_running = true;
 
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
+	uint64_t param_time = 0;
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
+	uint64_t status_time = 0;
 
-	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
+	struct vehicle_status_s status;
+	status_sub->update(&status_time, &status);
 
 	MavlinkCommandsStream commands_stream(this, _channel);
 
@@ -1993,14 +2020,14 @@ Mavlink::task_main(int argc, char *argv[])
 
 		hrt_abstime t = hrt_absolute_time();
 
-		if (param_sub->update(t)) {
+		if (param_sub->update(&param_time, nullptr)) {
 			/* parameters updated */
 			mavlink_update_system();
 		}
 
-		if (status_sub->update(t)) {
+		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
-			set_hil_enabled(status->hil_state == HIL_STATE_ON);
+			set_hil_enabled(status.hil_state == HIL_STATE_ON);
 		}
 
 		/* update commands stream */
@@ -2064,32 +2091,50 @@ 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;
+			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) {
-				/* write first part of buffer */
-				_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
-				message_buffer_mark_read(available);
+                // Reconstruct message from buffer
+
+				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 */
-				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);
 			}
 		}
 
@@ -2139,7 +2184,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);
 	}
@@ -2157,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[])
 	/* create the instance in task context */
 	Mavlink *instance = new Mavlink();
 
-	/* this will actually only return once MAVLink exits */
-	int res = instance->task_main(argc, argv);
+	int res;
 
-	/* delete instance on main thread end */
-	delete instance;
+	if (!instance) {
+
+		/* out of memory */
+		res = -ENOMEM;
+		warnx("OUT OF MEM");
+	} else {
+		/* this will actually only return once MAVLink exits */
+		res = instance->task_main(argc, argv);
+
+		/* delete instance on main thread end */
+		delete instance;
+	}
 
 	return res;
 }
@@ -2281,7 +2335,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_main.h b/src/modules/mavlink/mavlink_main.h
index 40edc4b85..6777d56c3 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,105 @@ 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();
+
+	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
-	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; }
+
+	MavlinkStream * get_streams() { return _streams; } const
 
 
 	/* 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);
+    
+    void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+    void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
+
+	/**
+	 * Count a transmision error
+	 */
+	void count_txerr();
 
 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 +306,19 @@ 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 */
+	perf_counter_t		_txerr_perf;			/**< TX error counter */
+
+	bool			_param_initialized;
+	param_t			_param_system_id;
+	param_t			_param_component_id;
+	param_t			_param_system_type;
+	param_t			_param_use_hil_gps;
 
 	/**
 	 * Send one parameter.
@@ -289,7 +326,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 +334,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 +342,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 +359,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);
@@ -344,7 +381,6 @@ private:
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
 	int configure_stream(const char *stream_name, const float rate);
-	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
 	int message_buffer_init(int size);
 
@@ -354,8 +390,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);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 0ceee32a1..472e2810d 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -74,6 +74,8 @@
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_range_finder.h>
 
+#include <systemlib/err.h>
+
 #include "mavlink_messages.h"
 
 
@@ -118,50 +120,77 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
 
-	if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
-		/* use main state when navigator is not active */
-		if (status->main_state == MAIN_STATE_MANUAL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+	switch (status->nav_state) {
+
+		case NAVIGATION_STATE_MANUAL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
+
+		case NAVIGATION_STATE_ACRO:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
+			break;
 
-		} else if (status->main_state == MAIN_STATE_ALTCTL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+		case NAVIGATION_STATE_ALTCTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
+			break;
 
-		} else if (status->main_state == MAIN_STATE_POSCTL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		case NAVIGATION_STATE_POSCTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+			break;
 
-		} else if (status->main_state == MAIN_STATE_AUTO) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		case NAVIGATION_STATE_AUTO_MISSION:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-
-		} else if (status->main_state == MAIN_STATE_ACRO) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
-		}
-
-	} else {
-		/* use navigation state when navigator is active */
-		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-
-		if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
+		case NAVIGATION_STATE_AUTO_LOITER:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
+		case NAVIGATION_STATE_AUTO_RTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
+		case NAVIGATION_STATE_LAND:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		}
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTGS:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
+			break;
+
+		case NAVIGATION_STATE_TERMINATION:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
+
 	}
 
 	*mavlink_custom_mode = custom_mode.data;
@@ -193,42 +222,57 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 class MavlinkStreamHeartbeat : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamHeartbeat::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "HEARTBEAT";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HEARTBEAT;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHeartbeat();
 	}
 
 private:
 	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
-
 	MavlinkOrbSubscription *pos_sp_triplet_sub;
-	struct position_setpoint_triplet_s *pos_sp_triplet;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		status = (struct vehicle_status_s *)status_sub->get_data();
-
 		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		(void)status_sub->update(t);
-		(void)pos_sp_triplet_sub->update(t);
+		struct vehicle_status_s status;
+		struct position_setpoint_triplet_s pos_sp_triplet;
+
+		/* 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 (!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;
 		uint32_t mavlink_custom_mode = 0;
-		get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 		mavlink_msg_heartbeat_send(_channel,
 					   mavlink_system.type,
@@ -236,7 +280,6 @@ protected:
 					   mavlink_base_mode,
 					   mavlink_custom_mode,
 					   mavlink_state);
-
 	}
 };
 
@@ -244,44 +287,55 @@ protected:
 class MavlinkStreamSysStatus : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamSysStatus::get_name_static();
+	}
+
+	static const char *get_name_static ()
 	{
 		return "SYS_STATUS";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SYS_STATUS;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamSysStatus();
 	}
 
 private:
 	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		status = (struct vehicle_status_s *)status_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
-		mavlink_msg_sys_status_send(_channel,
-						status->onboard_control_sensors_present,
-						status->onboard_control_sensors_enabled,
-						status->onboard_control_sensors_health,
-						status->load * 1000.0f,
-						status->battery_voltage * 1000.0f,
-						status->battery_current * 100.0f,
-						status->battery_remaining * 100.0f,
-						status->drop_rate_comm,
-						status->errors_comm,
-						status->errors_count1,
-						status->errors_count2,
-						status->errors_count3,
-						status->errors_count4);
+		struct vehicle_status_s status;
+
+		if (status_sub->update(&status)) {
+			mavlink_msg_sys_status_send(_channel,
+							status.onboard_control_sensors_present,
+							status.onboard_control_sensors_enabled,
+							status.onboard_control_sensors_health,
+							status.load * 1000.0f,
+							status.battery_voltage * 1000.0f,
+							status.battery_current * 100.0f,
+							status.battery_remaining * 100.0f,
+							status.drop_rate_comm,
+							status.errors_comm,
+							status.errors_count1,
+							status.errors_count2,
+							status.errors_count3,
+							status.errors_count4);
+		}
 	}
 };
 
@@ -289,23 +343,29 @@ protected:
 class MavlinkStreamHighresIMU : public MavlinkStream
 {
 public:
-	MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0)
+	const char *get_name() const
 	{
+		return MavlinkStreamHighresIMU::get_name_static();
 	}
 
-	const char *get_name()
+	static const char *get_name_static()
 	{
 		return "HIGHRES_IMU";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIGHRES_IMU;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHighresIMU();
 	}
 
 private:
 	MavlinkOrbSubscription *sensor_sub;
-	struct sensor_combined_s *sensor;
+	uint64_t sensor_time;
 
 	uint64_t accel_timestamp;
 	uint64_t gyro_timestamp;
@@ -313,48 +373,57 @@ private:
 	uint64_t baro_timestamp;
 
 protected:
+	explicit MavlinkStreamHighresIMU() : MavlinkStream(),
+		sensor_time(0),
+		accel_timestamp(0),
+		gyro_timestamp(0),
+		mag_timestamp(0),
+		baro_timestamp(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
-		sensor = (struct sensor_combined_s *)sensor_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (sensor_sub->update(t)) {
+		struct sensor_combined_s sensor;
+
+		if (sensor_sub->update(&sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
 
-			if (accel_timestamp != sensor->accelerometer_timestamp) {
+			if (accel_timestamp != sensor.accelerometer_timestamp) {
 				/* mark first three dimensions as changed */
 				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-				accel_timestamp = sensor->accelerometer_timestamp;
+				accel_timestamp = sensor.accelerometer_timestamp;
 			}
 
-			if (gyro_timestamp != sensor->timestamp) {
+			if (gyro_timestamp != sensor.timestamp) {
 				/* mark second group dimensions as changed */
 				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-				gyro_timestamp = sensor->timestamp;
+				gyro_timestamp = sensor.timestamp;
 			}
 
-			if (mag_timestamp != sensor->magnetometer_timestamp) {
+			if (mag_timestamp != sensor.magnetometer_timestamp) {
 				/* mark third group dimensions as changed */
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-				mag_timestamp = sensor->magnetometer_timestamp;
+				mag_timestamp = sensor.magnetometer_timestamp;
 			}
 
-			if (baro_timestamp != sensor->baro_timestamp) {
+			if (baro_timestamp != sensor.baro_timestamp) {
 				/* mark last group dimensions as changed */
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-				baro_timestamp = sensor->baro_timestamp;
+				baro_timestamp = sensor.baro_timestamp;
 			}
 
 			mavlink_msg_highres_imu_send(_channel,
-						     sensor->timestamp,
-						     sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
-						     sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
-						     sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
-						     sensor->baro_pres_mbar, sensor->differential_pressure_pa,
-						     sensor->baro_alt_meter, sensor->baro_temp_celcius,
+						     sensor.timestamp,
+						     sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2],
+						     sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2],
+						     sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2],
+						     sensor.baro_pres_mbar, sensor.differential_pressure_pa,
+						     sensor.baro_alt_meter, sensor.baro_temp_celcius,
 						     fields_updated);
 		}
 	}
@@ -364,34 +433,49 @@ protected:
 class MavlinkStreamAttitude : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitude::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ATTITUDE";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitude();
 	}
 
 private:
 	MavlinkOrbSubscription *att_sub;
-	struct vehicle_attitude_s *att;
+	uint64_t att_time;
 
 protected:
+	explicit MavlinkStreamAttitude() : MavlinkStream(),
+		att_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-		att = (struct vehicle_attitude_s *)att_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_sub->update(t)) {
+		struct vehicle_attitude_s att;
+
+		if (att_sub->update(&att_time, &att)) {
 			mavlink_msg_attitude_send(_channel,
-						  att->timestamp / 1000,
-						  att->roll, att->pitch, att->yaw,
-						  att->rollspeed, att->pitchspeed, att->yawspeed);
+						  att.timestamp / 1000,
+						  att.roll, att.pitch, att.yaw,
+						  att.rollspeed, att.pitchspeed, att.yawspeed);
 		}
 	}
 };
@@ -400,39 +484,54 @@ protected:
 class MavlinkStreamAttitudeQuaternion : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitudeQuaternion::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ATTITUDE_QUATERNION";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeQuaternion();
 	}
 
 private:
 	MavlinkOrbSubscription *att_sub;
-	struct vehicle_attitude_s *att;
+	uint64_t att_time;
 
 protected:
+	explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
+		att_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-		att = (struct vehicle_attitude_s *)att_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_sub->update(t)) {
+		struct vehicle_attitude_s att;
+
+		if (att_sub->update(&att_time, &att)) {
 			mavlink_msg_attitude_quaternion_send(_channel,
-							     att->timestamp / 1000,
-							     att->q[0],
-							     att->q[1],
-							     att->q[2],
-							     att->q[3],
-							     att->rollspeed,
-							     att->pitchspeed,
-							     att->yawspeed);
+							     att.timestamp / 1000,
+							     att.q[0],
+							     att.q[1],
+							     att.q[2],
+							     att.q[3],
+							     att.rollspeed,
+							     att.pitchspeed,
+							     att.yawspeed);
 		}
 	}
 };
@@ -441,71 +540,87 @@ protected:
 class MavlinkStreamVFRHUD : public MavlinkStream
 {
 public:
-	const char *get_name()
+
+	const char *get_name() const
+	{
+		return MavlinkStreamVFRHUD::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "VFR_HUD";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VFR_HUD;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamVFRHUD();
 	}
 
 private:
 	MavlinkOrbSubscription *att_sub;
-	struct vehicle_attitude_s *att;
+	uint64_t att_time;
 
 	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_global_position_s *pos;
+	uint64_t pos_time;
 
 	MavlinkOrbSubscription *armed_sub;
-	struct actuator_armed_s *armed;
+	uint64_t armed_time;
 
 	MavlinkOrbSubscription *act_sub;
-	struct actuator_controls_s *act;
+	uint64_t act_time;
 
 	MavlinkOrbSubscription *airspeed_sub;
-	struct airspeed_s *airspeed;
+	uint64_t airspeed_time;
 
 protected:
+	explicit MavlinkStreamVFRHUD() : MavlinkStream(),
+		att_time(0),
+		pos_time(0),
+		armed_time(0),
+		act_time(0),
+		airspeed_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-		att = (struct vehicle_attitude_s *)att_sub->get_data();
-
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
-		pos = (struct vehicle_global_position_s *)pos_sub->get_data();
-
 		armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
-		armed = (struct actuator_armed_s *)armed_sub->get_data();
-
 		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
-		act = (struct actuator_controls_s *)act_sub->get_data();
-
 		airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
-		airspeed = (struct airspeed_s *)airspeed_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = att_sub->update(t);
-		updated |= pos_sub->update(t);
-		updated |= armed_sub->update(t);
-		updated |= act_sub->update(t);
-		updated |= airspeed_sub->update(t);
+		struct vehicle_attitude_s att;
+		struct vehicle_global_position_s pos;
+		struct actuator_armed_s armed;
+		struct actuator_controls_s act;
+		struct airspeed_s airspeed;
+
+		bool updated = att_sub->update(&att_time, &att);
+		updated |= pos_sub->update(&pos_time, &pos);
+		updated |= armed_sub->update(&armed_time, &armed);
+		updated |= act_sub->update(&act_time, &act);
+		updated |= airspeed_sub->update(&airspeed_time, &airspeed);
 
 		if (updated) {
-			float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
-			uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
-			float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
+			float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
+			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+			float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
 
 			mavlink_msg_vfr_hud_send(_channel,
-						 airspeed->true_airspeed_m_s,
+						 airspeed.true_airspeed_m_s,
 						 groundspeed,
 						 heading,
 						 throttle,
-						 pos->alt,
-						 -pos->vel_d);
+						 pos.alt,
+						 -pos.vel_d);
 		}
 	}
 };
@@ -514,38 +629,53 @@ protected:
 class MavlinkStreamGPSRawInt : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamGPSRawInt::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "GPS_RAW_INT";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_RAW_INT;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSRawInt();
 	}
 
 private:
 	MavlinkOrbSubscription *gps_sub;
-	struct vehicle_gps_position_s *gps;
+	uint64_t gps_time;
 
 protected:
+	explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
+		gps_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
-		gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (gps_sub->update(t)) {
+		struct vehicle_gps_position_s gps;
+
+		if (gps_sub->update(&gps_time, &gps)) {
 			mavlink_msg_gps_raw_int_send(_channel,
 						     gps->timestamp_position,
 						     gps->fix_type,
 						     gps->lat,
 						     gps->lon,
 						     gps->alt,
-						     cm_uint16_from_m_float(gps->eph_m),
-						     cm_uint16_from_m_float(gps->epv_m),
+						     cm_uint16_from_m_float(gps->eph),
+						     cm_uint16_from_m_float(gps->epv),
 						     gps->vel_m_s * 100.0f,
 						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
 						     gps->satellites_used);
@@ -557,49 +687,64 @@ protected:
 class MavlinkStreamGlobalPositionInt : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamGlobalPositionInt::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "GLOBAL_POSITION_INT";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionInt();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_global_position_s *pos;
+	uint64_t pos_time;
 
 	MavlinkOrbSubscription *home_sub;
-	struct home_position_s *home;
+	uint64_t home_time;
 
 protected:
+	explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
+		pos_time(0),
+		home_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
-		pos = (struct vehicle_global_position_s *)pos_sub->get_data();
-
 		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
-		home = (struct home_position_s *)home_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = pos_sub->update(t);
-		updated |= home_sub->update(t);
+		struct vehicle_global_position_s pos;
+		struct home_position_s home;
+
+		bool updated = pos_sub->update(&pos_time, &pos);
+		updated |= home_sub->update(&home_time, &home);
 
 		if (updated) {
 			mavlink_msg_global_position_int_send(_channel,
-							     pos->timestamp / 1000,
-							     pos->lat * 1e7,
-							     pos->lon * 1e7,
-							     pos->alt * 1000.0f,
-							     (pos->alt - home->alt) * 1000.0f,
-							     pos->vel_n * 100.0f,
-							     pos->vel_e * 100.0f,
-							     pos->vel_d * 100.0f,
-							     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+							     pos.timestamp / 1000,
+							     pos.lat * 1e7,
+							     pos.lon * 1e7,
+							     pos.alt * 1000.0f,
+							     (pos.alt - home.alt) * 1000.0f,
+							     pos.vel_n * 100.0f,
+							     pos.vel_e * 100.0f,
+							     pos.vel_d * 100.0f,
+							     _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
 		}
 	}
 };
@@ -608,38 +753,53 @@ protected:
 class MavlinkStreamLocalPositionNED : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamLocalPositionNED::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "LOCAL_POSITION_NED";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionNED();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_local_position_s *pos;
+	uint64_t pos_time;
 
 protected:
+	explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
+		pos_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
-		pos = (struct vehicle_local_position_s *)pos_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sub->update(t)) {
+		struct vehicle_local_position_s pos;
+
+		if (pos_sub->update(&pos_time, &pos)) {
 			mavlink_msg_local_position_ned_send(_channel,
-							    pos->timestamp / 1000,
-							    pos->x,
-							    pos->y,
-							    pos->z,
-							    pos->vx,
-							    pos->vy,
-							    pos->vz);
+							    pos.timestamp / 1000,
+							    pos.x,
+							    pos.y,
+							    pos.z,
+							    pos.vx,
+							    pos.vy,
+							    pos.vz);
 		}
 	}
 };
@@ -649,38 +809,53 @@ protected:
 class MavlinkStreamViconPositionEstimate : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamViconPositionEstimate::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "VICON_POSITION_ESTIMATE";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamViconPositionEstimate();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_vicon_position_s *pos;
+	uint64_t pos_time;
 
 protected:
+	explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
+		pos_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
-		pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sub->update(t)) {
+		struct vehicle_vicon_position_s pos;
+
+		if (pos_sub->update(&pos_time, &pos)) {
 			mavlink_msg_vicon_position_estimate_send(_channel,
-								pos->timestamp / 1000,
-								pos->x,
-								pos->y,
-								pos->z,
-								pos->roll,
-								pos->pitch,
-								pos->yaw);
+								pos.timestamp / 1000,
+								pos.x,
+								pos.y,
+								pos.z,
+								pos.roll,
+								pos.pitch,
+								pos.yaw);
 		}
 	}
 };
@@ -689,70 +864,97 @@ protected:
 class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamGPSGlobalOrigin::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "GPS_GLOBAL_ORIGIN";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSGlobalOrigin();
 	}
 
 private:
 	MavlinkOrbSubscription *home_sub;
-	struct home_position_s *home;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
-		home = (struct home_position_s *)home_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-
 		/* we're sending the GPS home periodically to ensure the
 		 * the GCS does pick it up at one point */
 		if (home_sub->is_published()) {
-			home_sub->update(t);
+			struct home_position_s home;
 
-			mavlink_msg_gps_global_origin_send(_channel,
-							   (int32_t)(home->lat * 1e7),
-							   (int32_t)(home->lon * 1e7),
-							   (int32_t)(home->alt) * 1000.0f);
+			if (home_sub->update(&home)) {
+				mavlink_msg_gps_global_origin_send(_channel,
+								   (int32_t)(home.lat * 1e7),
+								   (int32_t)(home.lon * 1e7),
+								   (int32_t)(home.alt) * 1000.0f);
+			}
 		}
 	}
 };
 
-
+template <int N>
 class MavlinkStreamServoOutputRaw : public MavlinkStream
 {
 public:
-	MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
+	const char *get_name() const
 	{
-		sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
+		return MavlinkStreamServoOutputRaw<N>::get_name_static();
 	}
 
-	const char *get_name()
+	uint8_t get_id()
 	{
-		return _name;
+		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
 	}
 
-	MavlinkStream *new_instance()
+	static const char *get_name_static()
 	{
-		return new MavlinkStreamServoOutputRaw(_n);
+		switch (N) {
+			case 0:
+			return "SERVO_OUTPUT_RAW_0";
+
+			case 1:
+			return "SERVO_OUTPUT_RAW_1";
+
+			case 2:
+			return "SERVO_OUTPUT_RAW_2";
+
+			case 3:
+			return "SERVO_OUTPUT_RAW_3";
+		}
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamServoOutputRaw<N>();
 	}
 
 private:
 	MavlinkOrbSubscription *act_sub;
-	struct actuator_outputs_s *act;
-
-	char _name[20];
-	unsigned int _n;
+	uint64_t act_time;
 
 protected:
+	explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
+		act_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		orb_id_t act_topics[] = {
@@ -762,24 +964,25 @@ protected:
 			ORB_ID(actuator_outputs_3)
 		};
 
-		act_sub = mavlink->add_orb_subscription(act_topics[_n]);
-		act = (struct actuator_outputs_s *)act_sub->get_data();
+		act_sub = mavlink->add_orb_subscription(act_topics[N]);
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (act_sub->update(t)) {
+		struct actuator_outputs_s act;
+
+		if (act_sub->update(&act_time, &act)) {
 			mavlink_msg_servo_output_raw_send(_channel,
-							  act->timestamp / 1000,
-							  _n,
-							  act->output[0],
-							  act->output[1],
-							  act->output[2],
-							  act->output[3],
-							  act->output[4],
-							  act->output[5],
-							  act->output[6],
-							  act->output[7]);
+							  act.timestamp / 1000,
+							  N,
+							  act.output[0],
+							  act.output[1],
+							  act.output[2],
+							  act.output[3],
+							  act.output[4],
+							  act.output[5],
+							  act.output[6],
+							  act.output[7]);
 		}
 	}
 };
@@ -788,51 +991,66 @@ protected:
 class MavlinkStreamHILControls : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamHILControls::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "HIL_CONTROLS";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIL_CONTROLS;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHILControls();
 	}
 
 private:
 	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
+	uint64_t status_time;
 
 	MavlinkOrbSubscription *pos_sp_triplet_sub;
-	struct position_setpoint_triplet_s *pos_sp_triplet;
+	uint64_t pos_sp_triplet_time;
 
 	MavlinkOrbSubscription *act_sub;
-	struct actuator_outputs_s *act;
+	uint64_t act_time;
 
 protected:
+	explicit MavlinkStreamHILControls() : MavlinkStream(),
+		status_time(0),
+		pos_sp_triplet_time(0),
+		act_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		status = (struct vehicle_status_s *)status_sub->get_data();
-
 		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
-
 		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
-		act = (struct actuator_outputs_s *)act_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = act_sub->update(t);
-		(void)pos_sp_triplet_sub->update(t);
-		(void)status_sub->update(t);
+		struct vehicle_status_s status;
+		struct position_setpoint_triplet_s pos_sp_triplet;
+		struct actuator_outputs_s act;
 
-		if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
+		bool updated = act_sub->update(&act_time, &act);
+		updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
+		updated |= status_sub->update(&status_time, &status);
+
+		if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
 			uint32_t mavlink_custom_mode;
-			get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
 				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
@@ -861,7 +1079,7 @@ protected:
 					if (i < n) {
 						if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
 							/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
-							out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
 
 						} else {
 							/* send 0 when disarmed */
@@ -891,12 +1109,12 @@ protected:
 				for (unsigned i = 0; i < 8; i++) {
 					if (i != 3) {
 						/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
-						out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+						out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
 
 					} else {
 
 						/* scale fake PWM out 900..2100 us to 0..1 for throttle */
-						out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+						out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
 					}
 
 				}
@@ -915,37 +1133,47 @@ protected:
 class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "GLOBAL_POSITION_SETPOINT_INT";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionSetpointInt();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sp_triplet_sub;
-	struct position_setpoint_triplet_s *pos_sp_triplet;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		/* always send this message, even if it has not been updated */
-		pos_sp_triplet_sub->update(t);
-		mavlink_msg_global_position_setpoint_int_send(_channel,
-			MAV_FRAME_GLOBAL,
-			(int32_t)(pos_sp_triplet->current.lat * 1e7),
-			(int32_t)(pos_sp_triplet->current.lon * 1e7),
-			(int32_t)(pos_sp_triplet->current.alt * 1000),
-			(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
+		struct position_setpoint_triplet_s pos_sp_triplet;
+
+		if (pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+			mavlink_msg_global_position_setpoint_int_send(_channel,
+					MAV_FRAME_GLOBAL,
+					(int32_t)(pos_sp_triplet.current.lat * 1e7),
+					(int32_t)(pos_sp_triplet.current.lon * 1e7),
+					(int32_t)(pos_sp_triplet.current.alt * 1000),
+					(int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f));
+		}
 	}
 };
 
@@ -953,36 +1181,51 @@ protected:
 class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamLocalPositionSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "LOCAL_POSITION_SETPOINT";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionSetpoint();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sp_sub;
-	struct vehicle_local_position_setpoint_s *pos_sp;
+	uint64_t pos_sp_time;
 
 protected:
+	explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
+		pos_sp_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
-		pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sp_sub->update(t)) {
+		struct vehicle_local_position_setpoint_s pos_sp;
+
+		if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
 			mavlink_msg_local_position_setpoint_send(_channel,
 					MAV_FRAME_LOCAL_NED,
-					pos_sp->x,
-					pos_sp->y,
-					pos_sp->z,
-					pos_sp->yaw);
+					pos_sp.x,
+					pos_sp.y,
+					pos_sp.z,
+					pos_sp.yaw);
 		}
 	}
 };
@@ -991,36 +1234,51 @@ protected:
 class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawThrustSetpoint();
 	}
 
 private:
 	MavlinkOrbSubscription *att_sp_sub;
-	struct vehicle_attitude_setpoint_s *att_sp;
+	uint64_t att_sp_time;
 
 protected:
+	explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
+		att_sp_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
-		att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_sp_sub->update(t)) {
+		struct vehicle_attitude_setpoint_s att_sp;
+
+		if (att_sp_sub->update(&att_sp_time, &att_sp)) {
 			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
-					att_sp->timestamp / 1000,
-					att_sp->roll_body,
-					att_sp->pitch_body,
-					att_sp->yaw_body,
-					att_sp->thrust);
+					att_sp.timestamp / 1000,
+					att_sp.roll_body,
+					att_sp.pitch_body,
+					att_sp.yaw_body,
+					att_sp.thrust);
 		}
 	}
 };
@@ -1029,36 +1287,51 @@ protected:
 class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
 	}
 
 private:
 	MavlinkOrbSubscription *att_rates_sp_sub;
-	struct vehicle_rates_setpoint_s *att_rates_sp;
+	uint64_t att_rates_sp_time;
 
 protected:
+	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
+		att_rates_sp_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
-		att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_rates_sp_sub->update(t)) {
+		struct vehicle_rates_setpoint_s att_rates_sp;
+
+		if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
 			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
-					att_rates_sp->timestamp / 1000,
-					att_rates_sp->roll,
-					att_rates_sp->pitch,
-					att_rates_sp->yaw,
-					att_rates_sp->thrust);
+					att_rates_sp.timestamp / 1000,
+					att_rates_sp.roll,
+					att_rates_sp.pitch,
+					att_rates_sp.yaw,
+					att_rates_sp.thrust);
 		}
 	}
 };
@@ -1067,47 +1340,87 @@ protected:
 class MavlinkStreamRCChannelsRaw : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamRCChannelsRaw::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "RC_CHANNELS_RAW";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRCChannelsRaw();
 	}
 
 private:
 	MavlinkOrbSubscription *rc_sub;
-	struct rc_input_values *rc;
+	uint64_t rc_time;
 
 protected:
+	explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
+		rc_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
-		rc = (struct rc_input_values *)rc_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (rc_sub->update(t)) {
+		struct rc_input_values rc;
+
+		if (rc_sub->update(&rc_time, &rc)) {
 			const unsigned port_width = 8;
 
-			for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
+			// Deprecated message (but still needed for compatibility!)
+			for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
 				/* Channels are sent in MAVLink main loop at a fixed interval */
 				mavlink_msg_rc_channels_raw_send(_channel,
-								 rc->timestamp_publication / 1000,
+								 rc.timestamp_publication / 1000,
 								 i,
-								 (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
-								 rc->rssi);
+								 (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
+								 rc.rssi);
 			}
+
+			// 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),
+					((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
+					((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
+					((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
+					((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
+					((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
+					((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
+					((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
+					((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
+					((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
+					((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
+					((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
+					((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
+					((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.rssi);
 		}
 	}
 };
@@ -1116,36 +1429,51 @@ protected:
 class MavlinkStreamManualControl : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamManualControl::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "MANUAL_CONTROL";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MANUAL_CONTROL;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamManualControl();
 	}
 
 private:
 	MavlinkOrbSubscription *manual_sub;
-	struct manual_control_setpoint_s *manual;
+	uint64_t manual_time;
 
 protected:
+	explicit MavlinkStreamManualControl() : MavlinkStream(),
+		manual_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
-		manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (manual_sub->update(t)) {
+		struct manual_control_setpoint_s manual;
+
+		if (manual_sub->update(&manual_time, &manual)) {
 			mavlink_msg_manual_control_send(_channel,
 							mavlink_system.sysid,
-							manual->x * 1000,
-							manual->y * 1000,
-							manual->z * 1000,
-							manual->r * 1000,
+							manual.x * 1000,
+							manual.y * 1000,
+							manual.z * 1000,
+							manual.r * 1000,
 							0);
 		}
 	}
@@ -1155,37 +1483,52 @@ protected:
 class MavlinkStreamOpticalFlow : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamOpticalFlow::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "OPTICAL_FLOW";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamOpticalFlow();
 	}
 
 private:
 	MavlinkOrbSubscription *flow_sub;
-	struct optical_flow_s *flow;
+	uint64_t flow_time;
 
 protected:
+	explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
+		flow_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
-		flow = (struct optical_flow_s *)flow_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (flow_sub->update(t)) {
+		struct optical_flow_s flow;
+
+		if (flow_sub->update(&flow_time, &flow)) {
 			mavlink_msg_optical_flow_send(_channel,
-						      flow->timestamp,
-						      flow->sensor_id,
-						      flow->flow_raw_x, flow->flow_raw_y,
-						      flow->flow_comp_x_m, flow->flow_comp_y_m,
-						      flow->quality,
-						      flow->ground_distance_m);
+						      flow.timestamp,
+						      flow.sensor_id,
+						      flow.flow_raw_x, flow.flow_raw_y,
+						      flow.flow_comp_x_m, flow.flow_comp_y_m,
+						      flow.quality,
+						      flow.ground_distance_m);
 		}
 	}
 };
@@ -1193,47 +1536,62 @@ protected:
 class MavlinkStreamAttitudeControls : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitudeControls::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ATTITUDE_CONTROLS";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeControls();
 	}
 
 private:
 	MavlinkOrbSubscription *att_ctrl_sub;
-	struct actuator_controls_s *att_ctrl;
+	uint64_t att_ctrl_time;
 
 protected:
+	explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
+		att_ctrl_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-		att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_ctrl_sub->update(t)) {
+		struct actuator_controls_s att_ctrl;
+
+		if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) {
 			/* send, add spaces so that string buffer is at least 10 chars long */
 			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
+							   att_ctrl.timestamp / 1000,
 							   "rll ctrl    ",
-							   att_ctrl->control[0]);
+							   att_ctrl.control[0]);
 			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
+							   att_ctrl.timestamp / 1000,
 							   "ptch ctrl    ",
-							   att_ctrl->control[1]);
+							   att_ctrl.control[1]);
 			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
+							   att_ctrl.timestamp / 1000,
 							   "yaw ctrl     ",
-							   att_ctrl->control[2]);
+							   att_ctrl.control[2]);
 			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
+							   att_ctrl.timestamp / 1000,
 							   "thr ctrl     ",
-							   att_ctrl->control[3]);
+							   att_ctrl.control[3]);
 		}
 	}
 };
@@ -1241,37 +1599,52 @@ protected:
 class MavlinkStreamNamedValueFloat : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamNamedValueFloat::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "NAMED_VALUE_FLOAT";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamNamedValueFloat();
 	}
 
 private:
 	MavlinkOrbSubscription *debug_sub;
-	struct debug_key_value_s *debug;
+	uint64_t debug_time;
 
 protected:
+	explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
+		debug_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
-		debug = (struct debug_key_value_s *)debug_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (debug_sub->update(t)) {
+		struct debug_key_value_s debug;
+
+		if (debug_sub->update(&debug_time, &debug)) {
 			/* enforce null termination */
-			debug->key[sizeof(debug->key) - 1] = '\0';
+			debug.key[sizeof(debug.key) - 1] = '\0';
 
 			mavlink_msg_named_value_float_send(_channel,
-							   debug->timestamp_ms,
-							   debug->key,
-							   debug->value);
+							   debug.timestamp_ms,
+							   debug.key,
+							   debug.value);
 		}
 	}
 };
@@ -1279,33 +1652,42 @@ protected:
 class MavlinkStreamCameraCapture : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamCameraCapture::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "CAMERA_CAPTURE";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamCameraCapture();
 	}
 
 private:
 	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		status = (struct vehicle_status_s *)status_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		(void)status_sub->update(t);
+		struct vehicle_status_s status;
+		(void)status_sub->update(&status);
 
-		if (status->arming_state == ARMING_STATE_ARMED
-		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		if (status.arming_state == ARMING_STATE_ARMED
+		    || status.arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
 			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
@@ -1320,34 +1702,49 @@ protected:
 class MavlinkStreamDistanceSensor : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamDistanceSensor::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "DISTANCE_SENSOR";
 	}
 
-	MavlinkStream *new_instance()
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
+	}
+
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamDistanceSensor();
 	}
 
 private:
 	MavlinkOrbSubscription *range_sub;
-	struct range_finder_report *range;
+	uint64_t range_time;
 
 protected:
+	explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
+		range_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
-		range = (struct range_finder_report *)range_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (range_sub->update(t)) {
+		struct range_finder_report range;
+
+		if (range_sub->update(&range_time, &range)) {
 
 			uint8_t type;
 
-			switch (range->type) {
+			switch (range.type) {
 				case RANGE_FINDER_TYPE_LASER:
 				type = MAV_DISTANCE_SENSOR_LASER;
 				break;
@@ -1357,39 +1754,40 @@ protected:
 			uint8_t orientation = 0;
 			uint8_t covariance = 20;
 
-			mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
-				range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+			mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation,
+				range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance);
 		}
 	}
 };
 
-MavlinkStream *streams_list[] = {
-	new MavlinkStreamHeartbeat(),
-	new MavlinkStreamSysStatus(),
-	new MavlinkStreamHighresIMU(),
-	new MavlinkStreamAttitude(),
-	new MavlinkStreamAttitudeQuaternion(),
-	new MavlinkStreamVFRHUD(),
-	new MavlinkStreamGPSRawInt(),
-	new MavlinkStreamGlobalPositionInt(),
-	new MavlinkStreamLocalPositionNED(),
-	new MavlinkStreamGPSGlobalOrigin(),
-	new MavlinkStreamServoOutputRaw(0),
-	new MavlinkStreamServoOutputRaw(1),
-	new MavlinkStreamServoOutputRaw(2),
-	new MavlinkStreamServoOutputRaw(3),
-	new MavlinkStreamHILControls(),
-	new MavlinkStreamGlobalPositionSetpointInt(),
-	new MavlinkStreamLocalPositionSetpoint(),
-	new MavlinkStreamRollPitchYawThrustSetpoint(),
-	new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
-	new MavlinkStreamRCChannelsRaw(),
-	new MavlinkStreamManualControl(),
-	new MavlinkStreamOpticalFlow(),
-	new MavlinkStreamAttitudeControls(),
-	new MavlinkStreamNamedValueFloat(),
-	new MavlinkStreamCameraCapture(),
-	new MavlinkStreamDistanceSensor(),
-	new MavlinkStreamViconPositionEstimate(),
+
+StreamListItem *streams_list[] = {
+	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
+	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
+	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
+	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
+	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
+	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
+	new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
+	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
+	new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
+	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, &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),
+	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
+	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
+	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
+	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index b8823263a..ee64d0e42 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -43,6 +43,19 @@
 
 #include "mavlink_stream.h"
 
-extern MavlinkStream *streams_list[];
+class StreamListItem {
+
+public:
+	MavlinkStream* (*new_instance)();
+	const char* (*get_name)();
+
+	StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+		new_instance(inst),
+		get_name(name) {};
+
+	~StreamListItem() {};
+};
+
+extern StreamListItem *streams_list[];
 
 #endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 21d5219d3..901fa8f05 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -50,50 +50,55 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
 	_fd(orb_subscribe(_topic)),
 	_published(false),
 	_topic(topic),
-	_last_check(0),
 	next(nullptr)
 {
-	_data = malloc(topic->o_size);
-	memset(_data, 0, topic->o_size);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
 {
 	close(_fd);
-	free(_data);
 }
 
 orb_id_t
-MavlinkOrbSubscription::get_topic()
+MavlinkOrbSubscription::get_topic() const
 {
 	return _topic;
 }
 
-void *
-MavlinkOrbSubscription::get_data()
-{
-	return _data;
-}
-
 bool
-MavlinkOrbSubscription::update(const hrt_abstime t)
+MavlinkOrbSubscription::update(uint64_t *time, void* data)
 {
-	if (_last_check == t) {
-		/* already checked right now, return result of the check */
-		return _updated;
+	// TODO this is NOT atomic operation, we can get data newer than time
+	// if topic was published between orb_stat and orb_copy calls.
 
-	} else {
-		_last_check = t;
-		orb_check(_fd, &_updated);
+	uint64_t time_topic;
+	if (orb_stat(_fd, &time_topic)) {
+		/* error getting last topic publication time */
+		time_topic = 0;
+	}
+
+	if (orb_copy(_topic, _fd, data)) {
+		/* error copying topic data */
+		memset(data, 0, _topic->o_size);
+		return false;
 
-		if (_updated) {
-			orb_copy(_topic, _fd, _data);
-			_published = true;
+	} else {
+		/* data copied successfully */
+		_published = true;
+		if (time_topic != *time) {
+			*time = time_topic;
 			return true;
+
+		} else {
+			return false;
 		}
 	}
+}
 
-	return false;
+bool
+MavlinkOrbSubscription::update(void* data)
+{
+	return !orb_copy(_topic, _fd, data);
 }
 
 bool
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 8c09772c8..71efb43af 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -48,12 +48,26 @@
 class MavlinkOrbSubscription
 {
 public:
-	MavlinkOrbSubscription *next;	/*< pointer to next subscription in list */
+	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
 
 	MavlinkOrbSubscription(const orb_id_t topic);
 	~MavlinkOrbSubscription();
 
-	bool update(const hrt_abstime t);
+	/**
+	 * Check if subscription updated and get data.
+	 *
+	 * @return true only if topic was updated and data copied to buffer successfully.
+	 * If topic was not updated since last check it will return false but still copy the data.
+	 * If no data available data buffer will be filled with zeroes.
+	 */
+	bool update(uint64_t *time, void* data);
+
+	/**
+	 * Copy topic data to given buffer.
+	 *
+	 * @return true only if topic data copied successfully.
+	 */
+	bool update(void* data);
 
 	/**
 	 * Check if the topic has been published.
@@ -62,16 +76,12 @@ public:
 	 * @return true if the topic has been published at least once.
 	 */
 	bool is_published();
-	void *get_data();
-	orb_id_t get_topic();
+	orb_id_t get_topic() const;
 
 private:
-	const orb_id_t _topic;		/*< topic metadata */
-	int _fd;					/*< subscription handle */
-	bool _published;			/*< topic was ever published */
-	void *_data;				/*< pointer to data buffer */
-	hrt_abstime _last_check;	/*< time of last check */
-	bool _updated;				/*< updated on last check */
+	const orb_id_t _topic;		///< topic metadata
+	int _fd;			///< subscription handle
+	bool _published;		///< topic was ever published
 };
 
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 46da1a0be..53a1638a3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -106,12 +106,17 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
+	_telemetry_heartbeat_time(0),
+	_radio_status_available(false),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
 	_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 +155,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_manual_control(msg);
 		break;
 
+	case MAVLINK_MSG_ID_HEARTBEAT:
+		handle_message_heartbeat(msg);
+		break;
+
+	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+		handle_message_request_data_stream(msg);
+		break;
+
+	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
+		break;
+
 	default:
 		break;
 	}
@@ -411,6 +428,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	memset(&tstatus, 0, sizeof(tstatus));
 
 	tstatus.timestamp = hrt_absolute_time();
+	tstatus.heartbeat_time = _telemetry_heartbeat_time;
 	tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 	tstatus.rssi = rstatus.rssi;
 	tstatus.remote_rssi = rstatus.remrssi;
@@ -426,6 +444,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	} else {
 		orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
 	}
+
+	/* this means that heartbeats alone won't be published to the radio status no more */
+	_radio_status_available = true;
 }
 
 void
@@ -452,6 +473,54 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
+{
+	mavlink_heartbeat_t hb;
+	mavlink_msg_heartbeat_decode(msg, &hb);
+
+	/* ignore own heartbeats, accept only heartbeats from GCS */
+	if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+		_telemetry_heartbeat_time = hrt_absolute_time();
+
+		/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+		if (!_radio_status_available) {
+
+			struct telemetry_status_s tstatus;
+			memset(&tstatus, 0, sizeof(tstatus));
+
+			tstatus.timestamp = _telemetry_heartbeat_time;
+			tstatus.heartbeat_time = _telemetry_heartbeat_time;
+			tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+
+			if (_telemetry_status_pub < 0) {
+				_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+
+			} else {
+				orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+			}
+		}
+	}
+}
+
+void
+MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
+{
+	mavlink_request_data_stream_t req;
+	mavlink_msg_request_data_stream_decode(msg, &req);
+
+	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
+		float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
+
+		MavlinkStream *stream;
+		LL_FOREACH(_mavlink->get_streams(), stream) {
+			if (req.req_stream_id == stream->get_id()) {
+				_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
+			}
+		}
+	}
+}
+
+void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
 	mavlink_hil_sensor_t imu;
@@ -667,12 +736,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.lat = gps.lat;
 	hil_gps.lon = gps.lon;
 	hil_gps.alt = gps.alt;
-	hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
-	hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
+	hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
+	hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
 
 	hil_gps.timestamp_variance = timestamp;
 	hil_gps.s_variance_m_s = 5.0f;
-	hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
+	hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
 
 	hil_gps.timestamp_velocity = timestamp;
 	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
@@ -951,7 +1020,6 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
 	pthread_attr_setstacksize(&receiveloop_attr, 2900);
-
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 9ab84b58a..040a07480 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
@@ -112,6 +114,8 @@ private:
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
+	void handle_message_heartbeat(mavlink_message_t *msg);
+	void handle_message_request_data_stream(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
@@ -138,6 +142,8 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	hrt_abstime _telemetry_heartbeat_time;
+	bool _radio_status_available;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 2979d20de..69809a386 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -50,14 +50,6 @@ class MavlinkStream;
 
 class MavlinkStream
 {
-private:
-	hrt_abstime _last_sent;
-
-protected:
-	mavlink_channel_t _channel;
-	unsigned int _interval;
-
-	virtual void send(const hrt_abstime t) = 0;
 
 public:
 	MavlinkStream *next;
@@ -71,9 +63,20 @@ public:
 	 * @return 0 if updated / sent, -1 if unchanged
 	 */
 	int update(const hrt_abstime t);
-	virtual MavlinkStream *new_instance() = 0;
+	static MavlinkStream *new_instance();
+	static const char *get_name_static();
 	virtual void subscribe(Mavlink *mavlink) = 0;
-	virtual const char *get_name() = 0;
+	virtual const char *get_name() const = 0;
+	virtual uint8_t get_id() = 0;
+
+protected:
+	mavlink_channel_t _channel;
+	unsigned int _interval;
+
+	virtual void send(const hrt_abstime t) = 0;
+
+private:
+	hrt_abstime _last_sent;
 };
 
 
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index f532e26fe..a4d8bfbfb 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