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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

---
 src/modules/mavlink/mavlink_ftp.cpp | 13 ++++++++-----
 src/modules/mavlink/mavlink_ftp.h   | 25 +++++++++++++------------
 2 files changed, 21 insertions(+), 17 deletions(-)

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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


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

---
 makefiles/config_aerocore_default.mk               |  1 -
 makefiles/config_px4fmu-v1_default.mk              |  1 -
 makefiles/config_px4fmu-v2_default.mk              |  1 -
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp   | 72 +++++-----------------
 4 files changed, 16 insertions(+), 59 deletions(-)

(limited to 'src/modules')

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


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

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

(limited to 'src/modules')

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