Merge pull request #3398 from dogmaphobic/logHandler

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.h

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   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:
+	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),
@@ -1077,6 +1078,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);
@@ -1723,6 +1727,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,
@@ -375,6 +376,7 @@ private:
 	MavlinkMissionManager		*_mission_manager;
 	MavlinkParametersManager	*_parameters_manager;
 	MavlinkFTP			*_mavlink_ftp;
+	MavlinkLogHandler		*_mavlink_log_handler;
 
 	MAVLINK_MODE 		_mode;