diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp index 2d99cf6b69..1268ecb596 100644 --- a/src/modules/logger/log_writer.cpp +++ b/src/modules/logger/log_writer.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "log_writer.h" +#include "messages.h" #include #include @@ -241,17 +242,35 @@ void LogWriter::run() } } -bool LogWriter::write(void *ptr, size_t size) +bool LogWriter::write(void *ptr, size_t size, uint64_t dropout_start) { // Bytes available to write size_t available = _buffer_size - _count; + size_t dropout_size = 0; - if (size > available) { + if (dropout_start) { + dropout_size = sizeof(message_dropout_s); + } + + if (size + dropout_size > available) { // buffer overflow return false; } + if (dropout_start) { + //write dropout msg + message_dropout_s dropout_msg; + dropout_msg.duration = (uint16_t)(hrt_elapsed_time(&dropout_start) / 1000); + write_no_check(&dropout_msg, sizeof(dropout_msg)); + } + + write_no_check(ptr, size); + return true; +} + +void LogWriter::write_no_check(void *ptr, size_t size) +{ size_t n = _buffer_size - _head; // bytes to end of the buffer uint8_t *buffer_c = reinterpret_cast(ptr); @@ -270,7 +289,6 @@ bool LogWriter::write(void *ptr, size_t size) memcpy(&(_buffer[_head]), &(buffer_c[n]), p); _head = (_head + p) % _buffer_size; _count += size; - return true; } size_t LogWriter::get_read_ptr(void **ptr, bool *is_part) diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h index 115f4e03ff..1d230f5602 100644 --- a/src/modules/logger/log_writer.h +++ b/src/modules/logger/log_writer.h @@ -67,9 +67,10 @@ public: /** * Write data to be logged. The caller must call lock() before calling this. + * @param dropout_start timestamp when lastest dropout occured. 0 if no dropout at the moment. * @return true on success, false if not enough space in the buffer left */ - bool write(void *ptr, size_t size); + bool write(void *ptr, size_t size, uint64_t dropout_start = 0); void lock() { @@ -113,6 +114,11 @@ private: _count -= n; } + /** + * Write to the buffer but assuming there is enough space + */ + inline void write_no_check(void *ptr, size_t size); + /* 512 didn't seem to work properly, 4096 should match the FAT cluster size */ static constexpr size_t _min_write_chunk = 4096; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 034eece065..0ac8134b0b 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "logger.h" +#include "messages.h" #include #include @@ -282,74 +283,6 @@ void Logger::run_trampoline(int argc, char *argv[]) logger_task = -1; } -enum class MessageType : uint8_t { - FORMAT = 'F', - DATA = 'D', - INFO = 'I', - PARAMETER = 'P', - ADD_LOGGED_MSG = 'A', - REMOVE_LOGGED_MSG = 'R', - SYNC = 'S', - DROPOUT = 'O', -}; - -/* declare message data structs with byte alignment (no padding) */ -#pragma pack(push, 1) - -#define MSG_HEADER_LEN 3 //accounts for msg_size and msg_type - -/** first bytes of the file */ -struct message_file_header_s { - uint8_t magic[8]; - uint64_t timestamp; -}; - -struct message_format_s { - uint16_t msg_size; //size of message - MSG_HEADER_LEN - uint8_t msg_type = static_cast(MessageType::FORMAT); - - char format[2096]; -}; - -struct message_add_logged_s { - uint16_t msg_size; //size of message - MSG_HEADER_LEN - uint8_t msg_type = static_cast(MessageType::ADD_LOGGED_MSG); - - uint8_t multi_id; - uint16_t msg_id; - char message_name[255]; -}; - -struct message_remove_logged_s { - uint16_t msg_size; //size of message - MSG_HEADER_LEN - uint8_t msg_type = static_cast(MessageType::REMOVE_LOGGED_MSG); - - uint16_t msg_id; -}; -struct message_data_header_s { - uint16_t msg_size; //size of message - MSG_HEADER_LEN - uint8_t msg_type = static_cast(MessageType::DATA); - - uint16_t msg_id; -}; - -struct message_info_header_s { - uint16_t msg_size; //size of message - MSG_HEADER_LEN - uint8_t msg_type = static_cast(MessageType::INFO); - - uint8_t key_len; - char key[255]; -}; - -struct message_parameter_header_s { - uint16_t msg_size; - uint8_t msg_type = static_cast(MessageType::PARAMETER); - - uint8_t key_len; - char key[255]; -}; -#pragma pack(pop) - static constexpr size_t MAX_DATA_SIZE = 740; @@ -611,7 +544,7 @@ void Logger::run() //PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size); - if (_writer.write(buffer, msg_size)) { + if (_writer.write(buffer, msg_size, _dropout_start)) { #ifdef DBGPRINT total_bytes += msg_size; diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h new file mode 100644 index 0000000000..58fdd56a94 --- /dev/null +++ b/src/modules/logger/messages.h @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 + +enum class MessageType : uint8_t { + FORMAT = 'F', + DATA = 'D', + INFO = 'I', + PARAMETER = 'P', + ADD_LOGGED_MSG = 'A', + REMOVE_LOGGED_MSG = 'R', + SYNC = 'S', + DROPOUT = 'O', +}; + + +/* declare message data structs with byte alignment (no padding) */ +#pragma pack(push, 1) + +#define MSG_HEADER_LEN 3 //accounts for msg_size and msg_type + +/** first bytes of the file */ +struct message_file_header_s { + uint8_t magic[8]; + uint64_t timestamp; +}; + +struct message_format_s { + uint16_t msg_size; //size of message - MSG_HEADER_LEN + uint8_t msg_type = static_cast(MessageType::FORMAT); + + char format[2096]; +}; + +struct message_add_logged_s { + uint16_t msg_size; //size of message - MSG_HEADER_LEN + uint8_t msg_type = static_cast(MessageType::ADD_LOGGED_MSG); + + uint8_t multi_id; + uint16_t msg_id; + char message_name[255]; +}; + +struct message_remove_logged_s { + uint16_t msg_size; //size of message - MSG_HEADER_LEN + uint8_t msg_type = static_cast(MessageType::REMOVE_LOGGED_MSG); + + uint16_t msg_id; +}; + +struct message_sync_s { + uint16_t msg_size; //size of message - MSG_HEADER_LEN + uint8_t msg_type = static_cast(MessageType::SYNC); + + uint8_t sync_magic[8]; +}; + +struct message_dropout_s { + uint16_t msg_size = sizeof(uint16_t); //size of message - MSG_HEADER_LEN + uint8_t msg_type = static_cast(MessageType::DROPOUT); + + uint16_t duration; //in ms +}; + +struct message_data_header_s { + uint16_t msg_size; //size of message - MSG_HEADER_LEN + uint8_t msg_type = static_cast(MessageType::DATA); + + uint16_t msg_id; +}; + +struct message_info_header_s { + uint16_t msg_size; //size of message - MSG_HEADER_LEN + uint8_t msg_type = static_cast(MessageType::INFO); + + uint8_t key_len; + char key[255]; +}; + +struct message_parameter_header_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(MessageType::PARAMETER); + + uint8_t key_len; + char key[255]; +}; +#pragma pack(pop)