diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt index 7124eb4811..c57d85e993 100644 --- a/src/modules/logger/CMakeLists.txt +++ b/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 diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp index fae8b96747..88797660d2 100644 --- a/src/modules/logger/log_writer.cpp +++ b/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; + } +} + } } diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h index 4c5191513d..b84021f5e7 100644 --- a/src/modules/logger/log_writer.h +++ b/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; }; diff --git a/src/modules/logger/log_writer_mavlink.cpp b/src/modules/logger/log_writer_mavlink.cpp new file mode 100644 index 0000000000..c4b988c2d6 --- /dev/null +++ b/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 +#include +#include +#include + +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; +} + +} +} diff --git a/src/modules/logger/log_writer_mavlink.h b/src/modules/logger/log_writer_mavlink.h new file mode 100644 index 0000000000..5a086ebb81 --- /dev/null +++ b/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 +#include +#include + +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; +}; + +} +}