Add mavlink log messages handler

.gitignore

@@ -23,6 +23,7 @@ build_*/
 core
 cscope.out
 Firmware.sublime-workspace
+user.sublime*
 Images/*.bin
 Images/*.px4
 mavlink/include/mavlink/v0.9/

src/modules/mavlink/CMakeLists.txt

@@ -56,6 +56,7 @@ px4_add_module(
 		mavlink_rate_limiter.cpp
 		mavlink_receiver.cpp
 		mavlink_ftp.cpp
+		mavlink_log_handler.cpp
 	DEPENDS
 		platforms__common
 	)

src/modules/mavlink/mavlink_log_handler.cpp

@@ -0,0 +1,529 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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.
+ *
+ ****************************************************************************/
+
+/// @file mavlink_log_handler.h
+/// @author px4dev, Gus Grubba <mavlink@grubba.com>
+
+#include "mavlink_log_handler.h"
+#include "mavlink_main.h"
+#include <sys/stat.h>
+#include <time.h>
+
+#define MOUNTPOINT PX4_ROOTFSDIR "/fs/microsd"
+static const char *kLogRoot    = MOUNTPOINT "/log";
+static const char *kLogData    = MOUNTPOINT "/logdata.txt";
+static const char *kTmpData    = MOUNTPOINT "/$log$.txt";
+
+#ifdef __PX4_NUTTX
+    #define PX4LOG_REGULAR_FILE DTYPE_FILE
+    #define PX4LOG_DIRECTORY 	DTYPE_DIRECTORY
+#else
+    #define PX4LOG_REGULAR_FILE DT_REG
+    #define PX4LOG_DIRECTORY 	DT_DIR
+#endif
+
+// #define MAVLINK_LOG_HANDLER_VERBOSE
+
+#ifdef MAVLINK_LOG_HANDLER_VERBOSE
+#define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__)
+#else
+#define PX4LOG_WARN(fmt, ...)
+#endif
+
+//-------------------------------------------------------------------
+static bool
+stat_file(const char* file, uint32_t* date = 0, uint32_t* size = 0) {
+	struct stat st;
+	if (stat(file, &st) == 0) {
+		if (date) *date = st.st_mtime;
+		if (size) *size = st.st_size;
+		return true;
+	}
+	return false;
+}
+
+//-------------------------------------------------------------------
+MavlinkLogHandler *
+MavlinkLogHandler::new_instance(Mavlink *mavlink)
+{
+    return new MavlinkLogHandler(mavlink);
+}
+
+//-------------------------------------------------------------------
+MavlinkLogHandler::MavlinkLogHandler(Mavlink *mavlink)
+    : MavlinkStream(mavlink)
+    , _pLogHandlerHelper(0)
+{
+
+}
+
+//-------------------------------------------------------------------
+void
+MavlinkLogHandler::handle_message(const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
+		_log_request_list(msg);
+		break;
+	case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
+		_log_request_data(msg);
+		break;
+	case MAVLINK_MSG_ID_LOG_ERASE:
+		_log_request_erase(msg);
+		break;
+	case MAVLINK_MSG_ID_LOG_REQUEST_END:
+		_log_request_end(msg);
+		break;
+	}
+}
+
+//-------------------------------------------------------------------
+const char*
+MavlinkLogHandler::get_name(void) const
+{
+    return "MAVLINK_LOG_HANDLER";
+}
+
+//-------------------------------------------------------------------
+uint8_t
+MavlinkLogHandler::get_id(void)
+{
+    return MAVLINK_MSG_ID_LOG_ENTRY;
+}
+
+//-------------------------------------------------------------------
+unsigned
+MavlinkLogHandler::get_size(void)
+{
+	//-- Sending Log Entries
+	if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING) {
+		return MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	//-- Sending Log Data (contents of one of the log files)
+	} else if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA) {
+		return MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	//-- Idle
+	} else {
+		return 0;
+	}
+}
+
+//-------------------------------------------------------------------
+void
+MavlinkLogHandler::send(const hrt_abstime /*t*/)
+{
+	//-- Send log entry stream packets until buffer is full
+	while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size()) {
+		_log_send_listing();
+	};
+	//-- An arbitrary count of max log data packets in one go
+	int count = 100;
+	while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && --count) {
+		_log_send_data();
+	};
+}
+
+//-------------------------------------------------------------------
+void
+MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg)
+{
+	mavlink_log_request_list_t request;
+	mavlink_msg_log_request_list_decode(msg, &request);
+	//-- Clear any existing request
+	if(_pLogHandlerHelper)
+		delete _pLogHandlerHelper;
+	//-- Prepare new request
+	_pLogHandlerHelper = new LogListHelper;
+        if (_pLogHandlerHelper->log_count)
+        {
+        	//-- Define (and clamp) range
+		_pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1;
+		_pLogHandlerHelper->last_entry = request.end   < _pLogHandlerHelper->log_count ? request.end   : _pLogHandlerHelper->log_count - 1;
+        }
+        //-- Send first response (the rest, if any, is sent by the stream system)
+        if(_mavlink->get_free_tx_buf() > get_size())
+        	_log_send_listing();
+}
+
+//-------------------------------------------------------------------
+void
+MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg)
+{
+	//-- If we haven't listed, we can't do much
+	if(!_pLogHandlerHelper) {
+		PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested.\n");
+		return;
+	}
+	mavlink_log_request_data_t request;
+	mavlink_msg_log_request_data_decode(msg, &request);
+	//-- Does the requested log exist?
+	if (request.id >= _pLogHandlerHelper->log_count) {
+		PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %u but we only have %u.\n", request.id, _pLogHandlerHelper->log_count);
+		return;
+	}
+	//-- If we were sending log entries, stop it
+        _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
+        //-- Init send log dataset
+        _pLogHandlerHelper->current_log_filename[0] = 0;
+        _pLogHandlerHelper->current_log_index = request.id;
+	uint32_t time_utc = 0;
+	_pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename);
+        _pLogHandlerHelper->current_log_data_offset = request.ofs;
+        if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) {
+		_pLogHandlerHelper->current_log_data_remaining = 0;
+        } else {
+		_pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs;
+        }
+        if (_pLogHandlerHelper->current_log_data_remaining > request.count) {
+		_pLogHandlerHelper->current_log_data_remaining = request.count;
+        }
+        _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA;
+        //-- Send first chunck. Rest is sent by stream system.
+        if(_mavlink->get_free_tx_buf() > get_size())
+        	_log_send_data();
+}
+
+//-------------------------------------------------------------------
+void
+MavlinkLogHandler::_log_request_erase(const mavlink_message_t *msg)
+{
+	/*
+	mavlink_log_erase_t request;
+	mavlink_msg_log_erase_decode(msg, &request);
+	*/
+	if (_pLogHandlerHelper) {
+		delete _pLogHandlerHelper;
+		_pLogHandlerHelper = 0;
+	}
+	LogListHelper::delete_all(kLogRoot);
+}
+
+//-------------------------------------------------------------------
+void
+MavlinkLogHandler::_log_request_end(const mavlink_message_t *msg)
+{
+	if (_pLogHandlerHelper) {
+		delete _pLogHandlerHelper;
+		_pLogHandlerHelper = 0;
+	}
+}
+
+//-------------------------------------------------------------------
+void
+MavlinkLogHandler::_log_send_listing()
+{
+	mavlink_log_entry_t response;
+	_pLogHandlerHelper->get_entry(_pLogHandlerHelper->next_entry, response.size, response.time_utc);
+	response.id 		= _pLogHandlerHelper->next_entry;
+	response.num_logs	= _pLogHandlerHelper->log_count;
+	response.last_log_num	= _pLogHandlerHelper->last_entry;
+	_mavlink->send_message(MAVLINK_MSG_ID_LOG_ENTRY, &response);
+        //-- If we're done listing, flag it.
+        if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
+		_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
+        } else {
+		_pLogHandlerHelper->next_entry++;
+        }
+}
+
+//-------------------------------------------------------------------
+void
+MavlinkLogHandler::_log_send_data()
+{
+	mavlink_log_data_t response;
+	uint32_t len = _pLogHandlerHelper->current_log_data_remaining;
+	if (len > sizeof(response.data)) {
+		len = sizeof(response.data);
+	}
+	size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data);
+	if (read_size < sizeof(response.data)) {
+		memset(&response.data[read_size], 0, sizeof(response.data) - read_size);
+	}
+	response.ofs   = _pLogHandlerHelper->current_log_data_offset;
+	response.id    = _pLogHandlerHelper->current_log_index;
+	response.count = read_size;
+	_mavlink->send_message(MAVLINK_MSG_ID_LOG_DATA, &response);
+	_pLogHandlerHelper->current_log_data_offset    += read_size;
+	_pLogHandlerHelper->current_log_data_remaining -= read_size;
+	if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) {
+		_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
+	}
+}
+
+//-------------------------------------------------------------------
+LogListHelper::LogListHelper()
+	: next_entry(0)
+	, last_entry(0)
+	, log_count(0)
+	, current_status(LOG_HANDLER_LISTING)
+	, current_log_index(0)
+	, current_log_size(0)
+	, current_log_data_offset(0)
+	, current_log_data_remaining(0)
+{
+	_init();
+}
+
+//-------------------------------------------------------------------
+LogListHelper::~LogListHelper()
+{
+	// Remove log data files (if any)
+	unlink(kLogData);
+	unlink(kTmpData);
+}
+
+//-------------------------------------------------------------------
+bool
+LogListHelper::get_entry(int idx, uint32_t& size, uint32_t& date, char* filename)
+{
+	//-- Find log file in log list file created during init()
+	size = 0;
+	date = 0;
+	bool result = false;
+	//-- Open list of log files
+	FILE* f = ::fopen(kLogData, "r");
+	if (f) {
+		//--- Find requested entry
+		char line[160];
+		int count = 0;
+		while (fgets(line, sizeof(line), f)) {
+			//-- Found our "index"
+			if(count++ == idx) {
+				char file[128];
+				if(sscanf(line, "%u %u %s", &date, &size, file) == 3) {
+					if(filename) {
+						strcpy(filename, file);
+					}
+					result = true;
+					break;
+				}
+			}
+		}
+		fclose(f);
+	}
+	return result;
+}
+
+//-------------------------------------------------------------------
+size_t
+LogListHelper::get_log_data(uint8_t len, uint8_t* buffer)
+{
+	if(!current_log_filename[0]) 
+		return 0;
+	FILE* f = fopen(current_log_filename, "r");
+	if (!f) {
+		PX4LOG_WARN("MavlinkLogHandler::get_log_data Could not open %s\n", current_log_filename);
+		return 0;
+	}
+	if(fseek(f, current_log_data_offset, SEEK_SET)) {
+		fclose(f);
+		PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename);
+		return 0;
+	}
+	size_t result = fread(buffer, 1, len, f);
+	fclose(f);
+	return result;
+}
+
+//-------------------------------------------------------------------
+void
+LogListHelper::_init()
+{
+	/*
+	
+		When this helper is created, it scans the log directory
+		and collects all log files found into one file for easy,
+		subsequent access.
+	*/
+
+	current_log_filename[0] = 0;
+	// Remove old log data file (if any)
+	unlink(kLogData);
+    	// Open log directory
+	DIR *dp = opendir(kLogRoot);
+	if (dp == nullptr) {
+		// No log directory. Nothing to do.
+		return;
+	}
+    	// Create work file
+    	FILE* f = ::fopen(kTmpData, "w");
+	if (!f) {
+		PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData);
+		closedir(dp);
+		return;
+	}
+	// Scan directory and collect log files
+	struct dirent entry, *result = nullptr;
+	while (readdir_r(dp, &entry, &result) == 0) {
+		// no more entries?
+		if (result == nullptr) {
+			break;
+		}
+		if (entry.d_type == PX4LOG_DIRECTORY)
+		{
+			time_t tt;
+			char log_path[128];
+			snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name);
+		        if (_get_session_date(log_path, entry.d_name, tt)) {
+				_scan_logs(f, log_path, tt);
+			}
+		}
+	}
+	closedir(dp);
+	fclose(f);
+	// Rename temp file to data file
+	if (rename(kTmpData, kLogData)) {
+		PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s\n", kTmpData);
+		log_count = 0;
+	}
+}
+
+//-------------------------------------------------------------------
+bool
+LogListHelper::_get_session_date(const char* path, const char* dir, time_t& date)
+{
+	if(strlen(dir) > 4) {
+		// Convert "sess000" to 00:00 Jan 1 1970 (day per session)
+		if (strncmp(dir, "sess", 4) == 0) {
+			unsigned u;
+			if(sscanf(&dir[4], "%u", &u) == 1) {
+				date = u * 60 * 60 * 24;
+				return true;
+			}
+		} else {
+			if (stat_file(path, &date)) {
+				return true;
+			}
+			/* strptime not available for some reason
+			// Get date from directory name
+			struct tm tt;
+			if(strptime(dir, "%Y-%m-%d", &tt)) {
+				date = mktime(&tt);
+				return true;
+			}
+			*/
+		}
+	}
+	return false;
+}
+
+//-------------------------------------------------------------------
+void
+LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date)
+{
+	DIR *dp = opendir(dir);
+	if (dp) {
+		struct dirent entry, *result = nullptr;
+		while (readdir_r(dp, &entry, &result) == 0) {
+			// no more entries?
+			if (result == nullptr) {
+				break;
+			}
+			if (entry.d_type == PX4LOG_REGULAR_FILE) {
+				time_t  ldate = date;
+				uint32_t size = 0;
+				char log_file_path[128];
+				snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, entry.d_name);
+				if(_get_log_time_size(log_file_path, entry.d_name, ldate, size)) {
+					//-- Write entry out to list file
+					fprintf(f, "%u %u %s\n", ldate, size, log_file_path);
+					log_count++;
+				}
+			}
+		}
+	}
+}
+
+//-------------------------------------------------------------------
+bool
+LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& date, uint32_t& size)
+{
+	if(file && file[0] && strstr(file, ".px4log")) {
+		// Convert "log000" to 00:00 (minute per flight in session)
+		if (strncmp(file, "log", 3) == 0) {
+			unsigned u;
+			if(sscanf(&file[3], "%u", &u) == 1) {
+				date += (u * 60);
+				if (stat_file(path, 0, &size)) {
+					return true;
+				}
+			}
+		} else {
+			if (stat_file(path, &date, &size)) {
+				return true;
+			}
+			/* strptime not available for some reason
+			// Get time from file name
+			struct tm tt;
+			if(strptime(file, "%H_%M_%S", &tt)) {
+				date += mktime(&tt);
+				return true;
+			}
+			*/
+		}
+	}
+	return false;
+}
+
+//-------------------------------------------------------------------
+void
+LogListHelper::delete_all(const char* dir)
+{
+    	//-- Open log directory
+	DIR *dp = opendir(dir);
+	if (dp == nullptr) {
+		return;
+	}
+	struct dirent entry, *result = nullptr;
+	while (readdir_r(dp, &entry, &result) == 0) {
+		// no more entries?
+		if (result == nullptr) {
+			break;
+		}
+		if (entry.d_type == PX4LOG_DIRECTORY && entry.d_name[0] != '.') {
+			char log_path[128];
+			snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name);
+			LogListHelper::delete_all(log_path);
+			if(rmdir(log_path)) {
+				PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s\n", log_path);
+			}
+		}
+		if (entry.d_type == PX4LOG_REGULAR_FILE) {
+			char log_path[128];
+			snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name);
+			if(unlink(log_path)) {
+				PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s\n", log_path);
+			}
+		}
+	}
+	closedir(dp);
+}

src/modules/mavlink/mavlink_log_handler.h

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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_log_handler.h
+/// @author px4dev, Gus Grubba <mavlink@grubba.com>
+
+#include <dirent.h>
+#include <queue.h>
+#include <time.h>
+#include <stdio.h>
+#include <v1.0/mavlink_types.h>
+#include "mavlink_stream.h"
+
+class Mavlink;
+
+// Log Listing Helper
+class LogListHelper
+{
+public:
+	LogListHelper();
+	~LogListHelper();
+
+public:
+	static void delete_all(const char* dir);
+	
+public:
+
+	bool 	get_entry		(int idx, uint32_t& size, uint32_t& date, char* filename = 0);
+	size_t 	get_log_data		(uint8_t len, uint8_t* buffer);
+
+	enum {
+		LOG_HANDLER_IDLE,
+		LOG_HANDLER_LISTING,
+		LOG_HANDLER_SENDING_DATA
+	};
+
+	int 		next_entry;
+	int 		last_entry;
+	int 		log_count;
+
+	int		current_status;
+	uint16_t	current_log_index;
+	uint32_t 	current_log_size;
+	uint32_t	current_log_data_offset;
+	uint32_t	current_log_data_remaining;
+	char		current_log_filename[128];
+
+private:
+	void 	_init			();
+	bool 	_get_session_date	(const char* path, const char* dir, time_t& date);
+	void	_scan_logs		(FILE* f, const char* dir, time_t& date);
+	bool 	_get_log_time_size	(const char* path, const char* file, time_t& date, uint32_t& size);
+};
+
+// MAVLink LOG_* Message Handler
+class MavlinkLogHandler : public MavlinkStream
+{
+public:
+	/// @brief Contructor is only public so unit test code can new objects.
+	MavlinkLogHandler(Mavlink *mavlink);
+
+	static MavlinkLogHandler *new_instance(Mavlink *mavlink);
+
+	// Handle possible LOG message
+	void handle_message		(const mavlink_message_t *msg);
+
+	// Overrides from MavlinkStream
+	const char*	get_name	(void) const;
+	uint8_t 	get_id		(void);
+	unsigned	get_size	(void);
+	void 		send		(const hrt_abstime t);
+
+private:
+	void _log_message	(const mavlink_message_t *msg);
+	void _log_request_list	(const mavlink_message_t *msg);
+	void _log_request_data	(const mavlink_message_t *msg);
+	void _log_request_erase	(const mavlink_message_t *msg);
+	void _log_request_end	(const mavlink_message_t *msg);
+	void _log_send_listing	();
+	void _log_send_data	();
+
+private:
+	LogListHelper	*_pLogHandlerHelper;
+
+};

src/modules/mavlink/mavlink_main.cpp

@@ -147,6 +147,7 @@ Mavlink::Mavlink() :
 	_mission_manager(nullptr),
 	_parameters_manager(nullptr),
 	_mavlink_ftp(nullptr),
+	_mavlink_log_handler(nullptr),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_radio_id(0),
@@ -1032,6 +1033,9 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 	/* handle packet with ftp component */
 	_mavlink_ftp->handle_message(msg);
 
+	/* handle packet with log component */
+	_mavlink_log_handler->handle_message(msg);
+
 	if (get_forwarding_on()) {
 		/* forward any messages to other mavlink instances */
 		Mavlink::forward_message(msg, this);
@@ -1669,6 +1673,11 @@ Mavlink::task_main(int argc, char *argv[])
 	_mavlink_ftp->set_interval(interval_from_rate(80.0f));
 	LL_APPEND(_streams, _mavlink_ftp);
 
+	/* MAVLINK_Log_Handler */
+	_mavlink_log_handler = (MavlinkLogHandler *) MavlinkLogHandler::new_instance(this);
+	_mavlink_log_handler->set_interval(interval_from_rate(80.0f));
+	LL_APPEND(_streams, _mavlink_log_handler);
+
 	/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
 	 * remote requests rate. Rate specified here controls how much bandwidth we will reserve for
 	 * mission messages. */

src/modules/mavlink/mavlink_main.h

@@ -67,6 +67,7 @@
 #include "mavlink_mission.h"
 #include "mavlink_parameters.h"
 #include "mavlink_ftp.h"
+#include "mavlink_log_handler.h"
 
 enum Protocol {
 	SERIAL = 0,
@@ -369,6 +370,7 @@ private:
 	MavlinkMissionManager		*_mission_manager;
 	MavlinkParametersManager	*_parameters_manager;
 	MavlinkFTP			*_mavlink_ftp;
+	MavlinkLogHandler		*_mavlink_log_handler;
 
 	MAVLINK_MODE 		_mode;