logger: add mavlink write backend

src/modules/logger/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 		logger.cpp
 		log_writer.cpp
 		log_writer_file.cpp
+		log_writer_mavlink.cpp
 	DEPENDS
 		platforms__common
 		modules__uORB

src/modules/logger/log_writer.cpp

@@ -42,12 +42,20 @@ LogWriter::LogWriter(Backend backend, size_t file_buffer_size, unsigned int queu
 	: _backend(backend)
 {
 	if (backend & BackendFile) {
-		_log_writer_file = new LogWriterFile(file_buffer_size);
+		_log_writer_file_for_write = _log_writer_file = new LogWriterFile(file_buffer_size);
 
 		if (!_log_writer_file) {
 			PX4_ERR("LogWriterFile allocation failed");
 		}
 	}
+
+	if (backend & BackendMavlink) {
+		_log_writer_mavlink_for_write = _log_writer_mavlink = new LogWriterMavlink(queue_size);
+
+		if (!_log_writer_mavlink) {
+			PX4_ERR("LogWriterMavlink allocation failed");
+		}
+	}
 }
 
 bool LogWriter::init()
@@ -64,7 +72,13 @@ bool LogWriter::init()
 			PX4_ERR("failed to create writer thread (%i)", ret);
 			return false;
 		}
+	}
 
+	if (_log_writer_mavlink) {
+		if (!_log_writer_mavlink->init()) {
+			PX4_ERR("mavlink init failed");
+			return false;
+		}
 	}
 
 	return true;
@@ -75,6 +89,10 @@ LogWriter::~LogWriter()
 	if (_log_writer_file) {
 		delete(_log_writer_file);
 	}
+
+	if (_log_writer_mavlink) {
+		delete(_log_writer_mavlink);
+	}
 }
 
 bool LogWriter::is_started() const
@@ -85,6 +103,10 @@ bool LogWriter::is_started() const
 		ret = _log_writer_file->is_started();
 	}
 
+	if (_log_writer_mavlink) {
+		ret = ret || _log_writer_mavlink->is_started();
+	}
+
 	return ret;
 }
 
@@ -94,6 +116,10 @@ bool LogWriter::is_started(Backend backend) const
 		return _log_writer_file->is_started();
 	}
 
+	if (backend == BackendMavlink && _log_writer_mavlink) {
+		return _log_writer_mavlink->is_started();
+	}
+
 	return false;
 }
 
@@ -111,6 +137,20 @@ void LogWriter::stop_log_file()
 	}
 }
 
+void LogWriter::start_log_mavlink()
+{
+	if (_log_writer_mavlink) {
+		_log_writer_mavlink->start_log();
+	}
+}
+
+void LogWriter::stop_log_mavlink()
+{
+	if (_log_writer_mavlink) {
+		_log_writer_mavlink->stop_log();
+	}
+}
+
 void LogWriter::thread_stop()
 {
 	if (_log_writer_file) {
@@ -122,8 +162,12 @@ int LogWriter::write_message(void *ptr, size_t size, uint64_t dropout_start)
 {
 	int ret_file = 0, ret_mavlink = 0;
 
-	if (_log_writer_file) {
-		ret_file = _log_writer_file->write_message(ptr, size, dropout_start);
+	if (_log_writer_file_for_write) {
+		ret_file = _log_writer_file_for_write->write_message(ptr, size, dropout_start);
+	}
+
+	if (_log_writer_mavlink_for_write) {
+		ret_mavlink = _log_writer_mavlink_for_write->write_message(ptr, size);
 	}
 
 	// file backend errors takes precedence
@@ -134,5 +178,22 @@ int LogWriter::write_message(void *ptr, size_t size, uint64_t dropout_start)
 	return ret_mavlink;
 }
 
+void LogWriter::select_write_backend(Backend backend)
+{
+	if (backend & BackendFile) {
+		_log_writer_file_for_write = _log_writer_file;
+
+	} else {
+		_log_writer_file_for_write = nullptr;
+	}
+
+	if (backend & BackendMavlink) {
+		_log_writer_mavlink_for_write = _log_writer_mavlink;
+
+	} else {
+		_log_writer_mavlink_for_write = nullptr;
+	}
+}
+
 }
 }

src/modules/logger/log_writer.h

@@ -34,6 +34,7 @@
 #pragma once
 
 #include "log_writer_file.h"
+#include "log_writer_mavlink.h"
 
 namespace px4
 {
@@ -68,6 +69,10 @@ public:
 
 	void stop_log_file();
 
+	void start_log_mavlink();
+
+	void stop_log_mavlink();
+
 	/**
 	 * whether logging is currently active or not (any of the selected backends).
 	 */
@@ -86,6 +91,13 @@ public:
 	 */
 	int write_message(void *ptr, size_t size, uint64_t dropout_start = 0);
 
+	/**
+	 * Select a backend, so that future calls to write_message() only write to the selected
+	 * backend, until unselect_write_backend() is called.
+	 * @param backend
+	 */
+	void select_write_backend(Backend backend);
+	void unselect_write_backend() { select_write_backend(BackendAll); }
 
 	/* file logging methods */
 
@@ -125,24 +137,36 @@ public:
 		return 0;
 	}
 
+
 	/**
-	 * Indicate to the underlying backend whether the following writes() need a reliable
+	 * Indicate to the underlying backend whether future write_message() calls need a reliable
 	 * transfer. Needed for header integrity.
 	 */
 	void set_need_reliable_transfer(bool need_reliable)
 	{
 		if (_log_writer_file) { _log_writer_file->set_need_reliable_transfer(need_reliable); }
+
+		if (_log_writer_mavlink) { _log_writer_mavlink->set_need_reliable_transfer(need_reliable); }
 	}
 
 	bool need_reliable_transfer() const
 	{
 		if (_log_writer_file) { return _log_writer_file->need_reliable_transfer(); }
+
+		if (_log_writer_mavlink) { return _log_writer_mavlink->need_reliable_transfer(); }
+
 		return false;
 	}
 
 private:
 
 	LogWriterFile *_log_writer_file = nullptr;
+	LogWriterMavlink *_log_writer_mavlink = nullptr;
+
+	LogWriterFile *_log_writer_file_for_write =
+		nullptr; ///< pointer that is used for writing, to temporarily select write backends
+	LogWriterMavlink *_log_writer_mavlink_for_write = nullptr;
+
 	const Backend _backend;
 };
 

src/modules/logger/log_writer_mavlink.cpp

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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 "log_writer_mavlink.h"
+#include "messages.h"
+
+#include <drivers/drv_hrt.h>
+#include <mathlib/mathlib.h>
+#include <px4_log.h>
+#include <px4_posix.h>
+
+namespace px4
+{
+namespace logger
+{
+
+
+LogWriterMavlink::LogWriterMavlink(unsigned int queue_size) :
+	_queue_size(queue_size)
+{
+	_ulog_stream_data.length = 0;
+}
+
+bool LogWriterMavlink::init()
+{
+	return true;
+}
+
+LogWriterMavlink::~LogWriterMavlink()
+{
+	if (_ulog_stream_ack_sub >= 0) {
+		orb_unsubscribe(_ulog_stream_ack_sub);
+	}
+
+	if (_ulog_stream_pub) {
+		orb_unadvertise(_ulog_stream_pub);
+	}
+}
+
+void LogWriterMavlink::start_log()
+{
+	if (_ulog_stream_ack_sub == -1) {
+		_ulog_stream_ack_sub = orb_subscribe(ORB_ID(ulog_stream_ack));
+	}
+
+	// make sure we don't get any stale ack's by doing an orb_copy
+	ulog_stream_ack_s ack;
+	orb_copy(ORB_ID(ulog_stream_ack), _ulog_stream_ack_sub, &ack);
+	_ulog_stream_data.sequence = 0;
+	_ulog_stream_data.length = 0;
+	_ulog_stream_data.first_message_offset = 0;
+	_is_started = true;
+}
+
+void LogWriterMavlink::stop_log()
+{
+	_ulog_stream_data.length = 0;
+	_is_started = false;
+}
+
+int LogWriterMavlink::write_message(void *ptr, size_t size)
+{
+	if (!is_started()) {
+		return 0;
+	}
+
+	const uint8_t data_len = (uint8_t)sizeof(_ulog_stream_data.data);
+	uint8_t *ptr_data = (uint8_t *)ptr;
+
+	if (_ulog_stream_data.first_message_offset == 255) {
+		_ulog_stream_data.first_message_offset = _ulog_stream_data.length;
+	}
+
+	while (size > 0) {
+		size_t send_len = math::min((size_t)data_len - _ulog_stream_data.length, size);
+		memcpy(_ulog_stream_data.data + _ulog_stream_data.length, ptr_data, send_len);
+		_ulog_stream_data.length += send_len;
+		ptr_data += send_len;
+		size -= send_len;
+
+		if (_ulog_stream_data.length >= data_len) {
+			if (publish_message()) {
+				return -2;
+			}
+		}
+	}
+
+	return 0;
+}
+
+void LogWriterMavlink::set_need_reliable_transfer(bool need_reliable)
+{
+	if (!need_reliable && _need_reliable_transfer) {
+		if (_ulog_stream_data.length > 0) {
+			// make sure to send previous data using reliable transfer
+			publish_message();
+		}
+	}
+
+	_need_reliable_transfer = need_reliable;
+}
+
+int LogWriterMavlink::publish_message()
+{
+	_ulog_stream_data.timestamp = hrt_absolute_time();
+	_ulog_stream_data.flags = 0;
+
+	if (_need_reliable_transfer) {
+		_ulog_stream_data.flags = _ulog_stream_data.FLAGS_NEED_ACK;
+	}
+
+	if (_ulog_stream_pub == nullptr) {
+		_ulog_stream_pub = orb_advertise_queue(ORB_ID(ulog_stream), &_ulog_stream_data, _queue_size);
+
+	} else {
+		orb_publish(ORB_ID(ulog_stream), _ulog_stream_pub, &_ulog_stream_data);
+	}
+
+	if (_need_reliable_transfer) {
+		// we need to wait for an ack. Note that this blocks the main logger thread, so if a file logging
+		// is already running, it will miss samples.
+		px4_pollfd_struct_t fds[1];
+		fds[0].fd = _ulog_stream_ack_sub;
+		fds[0].events = POLLIN;
+		bool got_ack = false;
+		const int timeout_ms = ulog_stream_ack_s::ACK_TIMEOUT * ulog_stream_ack_s::ACK_MAX_TRIES;
+
+		hrt_abstime started = hrt_absolute_time();
+
+		do {
+			int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_ms);
+
+			if (ret <= 0) {
+				break;
+			}
+
+			if (fds[0].revents & POLLIN) {
+				ulog_stream_ack_s ack;
+				orb_copy(ORB_ID(ulog_stream_ack), _ulog_stream_ack_sub, &ack);
+
+				if (ack.sequence == _ulog_stream_data.sequence) {
+					got_ack = true;
+				}
+
+			} else {
+				break;
+			}
+		} while (!got_ack && hrt_elapsed_time(&started) / 1000 < timeout_ms);
+
+		if (!got_ack) {
+			PX4_ERR("Ack timeout. Stopping mavlink log");
+			stop_log();
+			return -2;
+		}
+
+		PX4_DEBUG("got ack in %i ms", (int)(hrt_elapsed_time(&started) / 1000));
+	}
+
+	_ulog_stream_data.sequence++;
+	_ulog_stream_data.length = 0;
+	_ulog_stream_data.first_message_offset = 255;
+	return 0;
+}
+
+}
+}

src/modules/logger/log_writer_mavlink.h

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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
+
+#include <stdint.h>
+#include <uORB/topics/ulog_stream.h>
+#include <uORB/topics/ulog_stream_ack.h>
+
+namespace px4
+{
+namespace logger
+{
+
+/**
+ * @class LogWriterMavlink
+ * Writes logging data to uORB, and then sent via mavlink
+ */
+class LogWriterMavlink
+{
+public:
+	LogWriterMavlink(unsigned int queue_size);
+	~LogWriterMavlink();
+
+	bool init();
+
+	void start_log();
+
+	void stop_log();
+
+	bool is_started() const { return _is_started; }
+
+	/** @see LogWriter::write_message() */
+	int write_message(void *ptr, size_t size);
+
+	void set_need_reliable_transfer(bool need_reliable);
+
+	bool need_reliable_transfer() const
+	{
+		return _need_reliable_transfer;
+	}
+
+private:
+
+	/** publish message, wait for ack if needed & reset message */
+	int publish_message();
+
+	ulog_stream_s _ulog_stream_data;
+	orb_advert_t _ulog_stream_pub = nullptr;
+	int _ulog_stream_ack_sub = -1;
+	bool _need_reliable_transfer = false;
+	bool _is_started = false;
+	const unsigned int _queue_size;
+};
+
+}
+}