From ccdaabc7fbe44b2b913a4615ede64ba1aca86663 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 7 Oct 2016 16:35:28 +0200 Subject: [PATCH] refactor logger: prepare for multiple write backends --- src/modules/logger/CMakeLists.txt | 1 + src/modules/logger/log_writer.cpp | 293 +++++------------------ src/modules/logger/log_writer.h | 92 ++++---- src/modules/logger/log_writer_file.cpp | 312 +++++++++++++++++++++++++ src/modules/logger/log_writer_file.h | 150 ++++++++++++ src/modules/logger/logger.cpp | 39 ++-- src/modules/logger/logger.h | 6 +- 7 files changed, 583 insertions(+), 310 deletions(-) create mode 100644 src/modules/logger/log_writer_file.cpp create mode 100644 src/modules/logger/log_writer_file.h diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt index 371afa9b57..7124eb4811 100644 --- a/src/modules/logger/CMakeLists.txt +++ b/src/modules/logger/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_module( SRCS logger.cpp log_writer.cpp + log_writer_file.cpp DEPENDS platforms__common modules__uORB diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp index d6d5f28550..ad1c942314 100644 --- a/src/modules/logger/log_writer.cpp +++ b/src/modules/logger/log_writer.cpp @@ -32,281 +32,100 @@ ****************************************************************************/ #include "log_writer.h" -#include "messages.h" -#include -#include - -#include namespace px4 { namespace logger { -constexpr size_t LogWriter::_min_write_chunk; - -LogWriter::LogWriter(size_t buffer_size) : - //We always write larger chunks (orb messages) to the buffer, so the buffer - //needs to be larger than the minimum write chunk (300 is somewhat arbitrary) - _buffer_size(math::max(buffer_size, _min_write_chunk + 300)) +LogWriter::LogWriter(Backend backend, size_t file_buffer_size) + : _backend(backend) { - pthread_mutex_init(&_mtx, nullptr); - pthread_cond_init(&_cv, nullptr); - /* allocate write performance counters */ - _perf_write = perf_alloc(PC_ELAPSED, "sd write"); - _perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync"); + if (backend & BackendFile) { + _log_writer_file = new LogWriterFile(file_buffer_size); + + if (!_log_writer_file) { + PX4_ERR("LogWriterFile allocation failed"); + } + } } bool LogWriter::init() { - if (_buffer) { - return true; + if (_log_writer_file) { + if (!_log_writer_file->init()) { + return false; + } } - _buffer = new uint8_t[_buffer_size]; - - return _buffer; + return true; } LogWriter::~LogWriter() { - pthread_mutex_destroy(&_mtx); - pthread_cond_destroy(&_cv); - perf_free(_perf_write); - perf_free(_perf_fsync); - - if (_buffer) { - delete[] _buffer; + if (_log_writer_file) { + delete(_log_writer_file); } } -void LogWriter::start_log(const char *filename) +bool LogWriter::is_started() const { - _fd = ::open(filename, O_CREAT | O_WRONLY, PX4_O_MODE_666); + bool ret = false; - if (_fd < 0) { - PX4_ERR("Can't open log file %s", filename); - _should_run = false; - return; - - } else { - PX4_INFO("Opened log file: %s", filename); - _should_run = true; - _running = true; + if (_log_writer_file) { + ret = _log_writer_file->is_started(); } - // Clear buffer and counters - _head = 0; - _count = 0; - _total_written = 0; - notify(); -} - -void LogWriter::stop_log() -{ - _should_run = false; - notify(); -} - -int LogWriter::thread_start(pthread_t &thread) -{ - pthread_attr_t thr_attr; - pthread_attr_init(&thr_attr); - - sched_param param; - /* low priority, as this is expensive disk I/O */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&thr_attr, ¶m); - - pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1024)); - - int ret = pthread_create(&thread, &thr_attr, &LogWriter::run_helper, this); - pthread_attr_destroy(&thr_attr); - return ret; } +bool LogWriter::is_started_file() const +{ + if (_log_writer_file) { + return _log_writer_file->is_started(); + } + + return false; +} + +void LogWriter::start_log_file(const char *filename) +{ + if (_log_writer_file) { + _log_writer_file->start_log(filename); + } +} + +void LogWriter::stop_log_file() +{ + if (_log_writer_file) { + _log_writer_file->stop_log(); + } +} + +int LogWriter::thread_start(pthread_t &thread) +{ + if (_log_writer_file) { + return _log_writer_file->thread_start(thread); + } + + return 0; +} + void LogWriter::thread_stop() { - // this will terminate the main loop of the writer thread - _exit_thread = true; - _should_run = false; -} - -void *LogWriter::run_helper(void *context) -{ - px4_prctl(PR_SET_NAME, "log_writer", px4_getpid()); - - reinterpret_cast(context)->run(); - return nullptr; -} - -void LogWriter::run() -{ - while (!_exit_thread) { - // Outer endless loop - // Wait for _should_run flag - while (!_exit_thread) { - bool start = false; - pthread_mutex_lock(&_mtx); - pthread_cond_wait(&_cv, &_mtx); - start = _should_run; - pthread_mutex_unlock(&_mtx); - - if (start) { - break; - } - } - - int poll_count = 0; - int written = 0; - - while (true) { - size_t available = 0; - void *read_ptr = nullptr; - bool is_part = false; - - /* lock _buffer - * wait for sufficient data, cycle on notify() - */ - pthread_mutex_lock(&_mtx); - - while (true) { - available = get_read_ptr(&read_ptr, &is_part); - - /* if sufficient data available or partial read or terminating, exit this wait loop */ - if ((available >= _min_write_chunk) || is_part || !_should_run) { - /* GOTO end of block */ - break; - } - - /* wait for a call to notify() - * this call unlocks the mutex while waiting, and returns with the mutex locked - */ - pthread_cond_wait(&_cv, &_mtx); - } - - pthread_mutex_unlock(&_mtx); - written = 0; - - if (available > 0) { - perf_begin(_perf_write); - written = ::write(_fd, read_ptr, available); - perf_end(_perf_write); - - /* call fsync periodically to minimize potential loss of data */ - if (++poll_count >= 100) { - perf_begin(_perf_fsync); - ::fsync(_fd); - perf_end(_perf_fsync); - poll_count = 0; - } - - if (written < 0) { - PX4_WARN("error writing log file"); - _should_run = false; - /* GOTO end of block */ - break; - } - - pthread_mutex_lock(&_mtx); - /* subtract bytes written from number in _buffer (_count -= written) */ - mark_read(written); - pthread_mutex_unlock(&_mtx); - - _total_written += written; - } - - if (!_should_run && written == static_cast(available) && !is_part) { - // Stop only when all data written - _running = false; - _head = 0; - _count = 0; - - if (_fd >= 0) { - int res = ::close(_fd); - _fd = -1; - - if (res) { - PX4_WARN("error closing log file"); - - } else { - PX4_INFO("closed logfile, bytes written: %zu", _total_written); - } - } - - break; - } - } + if (_log_writer_file) { + _log_writer_file->thread_stop(); } } 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 (dropout_start) { - dropout_size = sizeof(ulog_message_dropout_s); + if (_log_writer_file) { + return _log_writer_file->write(ptr, size, dropout_start); } - if (size + dropout_size > available) { - // buffer overflow - return false; - } - - if (dropout_start) { - //write dropout msg - ulog_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); - - if (size > n) { - // Message goes over the end of the buffer - memcpy(&(_buffer[_head]), buffer_c, n); - _head = 0; - - } else { - n = 0; - } - - // now: n = bytes already written - size_t p = size - n; // number of bytes to write - memcpy(&(_buffer[_head]), &(buffer_c[n]), p); - _head = (_head + p) % _buffer_size; - _count += size; -} - -size_t LogWriter::get_read_ptr(void **ptr, bool *is_part) -{ - // bytes available to read - int read_ptr = _head - _count; - - if (read_ptr < 0) { - read_ptr += _buffer_size; - *ptr = &_buffer[read_ptr]; - *is_part = true; - return _buffer_size - read_ptr; - - } else { - *ptr = &_buffer[read_ptr]; - *is_part = false; - return _count; - } -} - } } diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h index 1285685da1..98ee7b356a 100644 --- a/src/modules/logger/log_writer.h +++ b/src/modules/logger/log_writer.h @@ -33,25 +33,33 @@ #pragma once -#include -#include -#include -#include -#include +#include "log_writer_file.h" namespace px4 { namespace logger { +/** + * @class LogWriter + * Manages starting, stopping & writing of logged data using the configured backend. + */ class LogWriter { public: - LogWriter(size_t buffer_size); + + /** bitfield to specify a backend */ + typedef uint8_t Backend; + static constexpr Backend BackendFile = 1 << 0; + static constexpr Backend BackendAll = BackendFile; + + LogWriter(Backend backend, size_t file_buffer_size); ~LogWriter(); bool init(); + Backend backend() const { return _backend; } + /** * start the thread * @param thread will be set to the created thread on success @@ -61,14 +69,19 @@ public: void thread_stop(); - void start_log(const char *filename); + void start_log_file(const char *filename); - void stop_log(); + void stop_log_file(); /** * whether logging is currently active or not. */ - bool is_started() const { return _should_run; } + bool is_started() const; + + /** + * whether file logging is currently active or not. + */ + bool is_started_file() const; /** * Write data to be logged. The caller must call lock() before calling this. @@ -77,70 +90,51 @@ public: */ bool write(void *ptr, size_t size, uint64_t dropout_start = 0); + + /* file logging methods */ + void lock() { - pthread_mutex_lock(&_mtx); + if (_log_writer_file) { _log_writer_file->lock(); } } void unlock() { - pthread_mutex_unlock(&_mtx); + if (_log_writer_file) { _log_writer_file->unlock(); } } void notify() { - pthread_cond_broadcast(&_cv); + if (_log_writer_file) { _log_writer_file->notify(); } } - size_t get_total_written() const + size_t get_total_written_file() const { - return _total_written; + if (_log_writer_file) { return _log_writer_file->get_total_written(); } + + return 0; } - size_t get_buffer_size() const + size_t get_buffer_size_file() const { - return _buffer_size; + if (_log_writer_file) { return _log_writer_file->get_buffer_size(); } + + return 0; } - size_t get_buffer_fill_count() const + size_t get_buffer_fill_count_file() const { - return _count; + if (_log_writer_file) { return _log_writer_file->get_buffer_fill_count(); } + + return 0; } private: - static void *run_helper(void *); - void run(); - - size_t get_read_ptr(void **ptr, bool *is_part); - - void mark_read(size_t n) - { - _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; - - int _fd = -1; - uint8_t *_buffer = nullptr; - const size_t _buffer_size; - size_t _head = 0; ///< next position to write to - size_t _count = 0; ///< number of bytes in _buffer to be written - size_t _total_written = 0; - bool _should_run = false; - bool _running = false; - bool _exit_thread = false; - pthread_mutex_t _mtx; - pthread_cond_t _cv; - perf_counter_t _perf_write; - perf_counter_t _perf_fsync; + LogWriterFile *_log_writer_file = nullptr; + const Backend _backend; }; + } } diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp new file mode 100644 index 0000000000..1180f4fbd3 --- /dev/null +++ b/src/modules/logger/log_writer_file.cpp @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * 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_file.h" +#include "messages.h" +#include +#include + +#include + +namespace px4 +{ +namespace logger +{ +constexpr size_t LogWriterFile::_min_write_chunk; + + +LogWriterFile::LogWriterFile(size_t buffer_size) : + //We always write larger chunks (orb messages) to the buffer, so the buffer + //needs to be larger than the minimum write chunk (300 is somewhat arbitrary) + _buffer_size(math::max(buffer_size, _min_write_chunk + 300)) +{ + pthread_mutex_init(&_mtx, nullptr); + pthread_cond_init(&_cv, nullptr); + /* allocate write performance counters */ + _perf_write = perf_alloc(PC_ELAPSED, "sd write"); + _perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync"); +} + +bool LogWriterFile::init() +{ + if (_buffer) { + return true; + } + + _buffer = new uint8_t[_buffer_size]; + + return _buffer; +} + +LogWriterFile::~LogWriterFile() +{ + pthread_mutex_destroy(&_mtx); + pthread_cond_destroy(&_cv); + perf_free(_perf_write); + perf_free(_perf_fsync); + + if (_buffer) { + delete[] _buffer; + } +} + +void LogWriterFile::start_log(const char *filename) +{ + _fd = ::open(filename, O_CREAT | O_WRONLY, PX4_O_MODE_666); + + if (_fd < 0) { + PX4_ERR("Can't open log file %s", filename); + _should_run = false; + return; + + } else { + PX4_INFO("Opened log file: %s", filename); + _should_run = true; + _running = true; + } + + // Clear buffer and counters + _head = 0; + _count = 0; + _total_written = 0; + notify(); +} + +void LogWriterFile::stop_log() +{ + _should_run = false; + notify(); +} + +int LogWriterFile::thread_start(pthread_t &thread) +{ + pthread_attr_t thr_attr; + pthread_attr_init(&thr_attr); + + sched_param param; + /* low priority, as this is expensive disk I/O */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; + (void)pthread_attr_setschedparam(&thr_attr, ¶m); + + pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1024)); + + int ret = pthread_create(&thread, &thr_attr, &LogWriterFile::run_helper, this); + pthread_attr_destroy(&thr_attr); + + return ret; +} + +void LogWriterFile::thread_stop() +{ + // this will terminate the main loop of the writer thread + _exit_thread = true; + _should_run = false; +} + +void *LogWriterFile::run_helper(void *context) +{ + px4_prctl(PR_SET_NAME, "log_writer_file", px4_getpid()); + + reinterpret_cast(context)->run(); + return nullptr; +} + +void LogWriterFile::run() +{ + while (!_exit_thread) { + // Outer endless loop + // Wait for _should_run flag + while (!_exit_thread) { + bool start = false; + pthread_mutex_lock(&_mtx); + pthread_cond_wait(&_cv, &_mtx); + start = _should_run; + pthread_mutex_unlock(&_mtx); + + if (start) { + break; + } + } + + int poll_count = 0; + int written = 0; + + while (true) { + size_t available = 0; + void *read_ptr = nullptr; + bool is_part = false; + + /* lock _buffer + * wait for sufficient data, cycle on notify() + */ + pthread_mutex_lock(&_mtx); + + while (true) { + available = get_read_ptr(&read_ptr, &is_part); + + /* if sufficient data available or partial read or terminating, exit this wait loop */ + if ((available >= _min_write_chunk) || is_part || !_should_run) { + /* GOTO end of block */ + break; + } + + /* wait for a call to notify() + * this call unlocks the mutex while waiting, and returns with the mutex locked + */ + pthread_cond_wait(&_cv, &_mtx); + } + + pthread_mutex_unlock(&_mtx); + written = 0; + + if (available > 0) { + perf_begin(_perf_write); + written = ::write(_fd, read_ptr, available); + perf_end(_perf_write); + + /* call fsync periodically to minimize potential loss of data */ + if (++poll_count >= 100) { + perf_begin(_perf_fsync); + ::fsync(_fd); + perf_end(_perf_fsync); + poll_count = 0; + } + + if (written < 0) { + PX4_WARN("error writing log file"); + _should_run = false; + /* GOTO end of block */ + break; + } + + pthread_mutex_lock(&_mtx); + /* subtract bytes written from number in _buffer (_count -= written) */ + mark_read(written); + pthread_mutex_unlock(&_mtx); + + _total_written += written; + } + + if (!_should_run && written == static_cast(available) && !is_part) { + // Stop only when all data written + _running = false; + _head = 0; + _count = 0; + + if (_fd >= 0) { + int res = ::close(_fd); + _fd = -1; + + if (res) { + PX4_WARN("error closing log file"); + + } else { + PX4_INFO("closed logfile, bytes written: %zu", _total_written); + } + } + + break; + } + } + } +} + +bool LogWriterFile::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 (dropout_start) { + dropout_size = sizeof(ulog_message_dropout_s); + } + + if (size + dropout_size > available) { + // buffer overflow + return false; + } + + if (dropout_start) { + //write dropout msg + ulog_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 LogWriterFile::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); + + if (size > n) { + // Message goes over the end of the buffer + memcpy(&(_buffer[_head]), buffer_c, n); + _head = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + size_t p = size - n; // number of bytes to write + memcpy(&(_buffer[_head]), &(buffer_c[n]), p); + _head = (_head + p) % _buffer_size; + _count += size; +} + +size_t LogWriterFile::get_read_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int read_ptr = _head - _count; + + if (read_ptr < 0) { + read_ptr += _buffer_size; + *ptr = &_buffer[read_ptr]; + *is_part = true; + return _buffer_size - read_ptr; + + } else { + *ptr = &_buffer[read_ptr]; + *is_part = false; + return _count; + } +} + +} +} diff --git a/src/modules/logger/log_writer_file.h b/src/modules/logger/log_writer_file.h new file mode 100644 index 0000000000..970c04d52e --- /dev/null +++ b/src/modules/logger/log_writer_file.h @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * 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 +#include +#include + +namespace px4 +{ +namespace logger +{ + +/** + * @class LogWriter + * Writes logging data to a file + */ +class LogWriterFile +{ +public: + LogWriterFile(size_t buffer_size); + ~LogWriterFile(); + + bool init(); + + /** + * start the thread + * @param thread will be set to the created thread on success + * @return 0 on success, error number otherwise (@see pthread_create) + */ + int thread_start(pthread_t &thread); + + void thread_stop(); + + void start_log(const char *filename); + + void stop_log(); + + /** + * whether logging is currently active or not. + */ + bool is_started() const { return _should_run; } + + /** + * 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, uint64_t dropout_start = 0); + + void lock() + { + pthread_mutex_lock(&_mtx); + } + + void unlock() + { + pthread_mutex_unlock(&_mtx); + } + + void notify() + { + pthread_cond_broadcast(&_cv); + } + + size_t get_total_written() const + { + return _total_written; + } + + size_t get_buffer_size() const + { + return _buffer_size; + } + + size_t get_buffer_fill_count() const + { + return _count; + } + +private: + static void *run_helper(void *); + + void run(); + + size_t get_read_ptr(void **ptr, bool *is_part); + + void mark_read(size_t n) + { + _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; + + int _fd = -1; + uint8_t *_buffer = nullptr; + const size_t _buffer_size; + size_t _head = 0; ///< next position to write to + size_t _count = 0; ///< number of bytes in _buffer to be written + size_t _total_written = 0; + bool _should_run = false; + bool _running = false; + bool _exit_thread = false; + pthread_mutex_t _mtx; + pthread_cond_t _cv; + perf_counter_t _perf_write; + perf_counter_t _perf_fsync; +}; + +} +} diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 0de68d20bd..2f42a2071b 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -219,7 +219,6 @@ void Logger::status() PX4_INFO("Running, but not logging"); } else { - PX4_INFO("Running"); print_statistics(); } } @@ -229,14 +228,16 @@ void Logger::print_statistics() return; } - float kibibytes = _writer.get_total_written() / 1024.0f; + PX4_INFO("File Logging Running"); + + float kibibytes = _writer.get_total_written_file() / 1024.0f; float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f; PX4_INFO("Log file: %s/%s", _log_dir, _log_file_name); PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds)); PX4_INFO("Since last status: dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B", - _write_dropouts, (double)_max_dropout_duration, _high_water, _writer.get_buffer_size()); + _write_dropouts, (double)_max_dropout_duration, _high_water, _writer.get_buffer_size_file()); _high_water = 0; _write_dropouts = 0; _max_dropout_duration = 0.f; @@ -308,7 +309,7 @@ void Logger::run_trampoline(int argc, char *argv[]) return; } - logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start, + logger_ptr = new Logger(LogWriter::BackendFile, log_buffer_size, log_interval, log_on_start, log_until_shutdown, log_name_timestamp); #if defined(DBGPRINT) && defined(__PX4_NUTTX) @@ -335,13 +336,13 @@ void Logger::run_trampoline(int argc, char *argv[]) } -Logger::Logger(size_t buffer_size, uint32_t log_interval, bool log_on_start, +Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, bool log_on_start, bool log_until_shutdown, bool log_name_timestamp) : _arm_override(false), _log_on_start(log_on_start), _log_until_shutdown(log_until_shutdown), _log_name_timestamp(log_name_timestamp), - _writer(buffer_size), + _writer(backend, buffer_size), _log_interval(log_interval) { _log_utc_offset = param_find("SDLOG_UTC_OFFSET"); @@ -657,7 +658,7 @@ void Logger::run() // we start logging immediately // the case where we wait with logging until vehicle is armed is handled below if (_log_on_start) { - start_log(); + start_log_file(); } /* init the update timer */ @@ -681,7 +682,7 @@ void Logger::run() _was_armed = armed; if (armed) { - start_log(); + start_log_file(); #ifdef DBGPRINT timer_start = hrt_absolute_time(); @@ -689,7 +690,7 @@ void Logger::run() #endif /* DBGPRINT */ } else { - stop_log(); + stop_log_file(); } } } @@ -765,8 +766,8 @@ void Logger::run() } } - if (!_dropout_start && _writer.get_buffer_fill_count() > _high_water) { - _high_water = _writer.get_buffer_fill_count(); + if (!_dropout_start && _writer.get_buffer_fill_count_file() > _high_water) { + _high_water = _writer.get_buffer_fill_count_file(); } /* release the log buffer */ @@ -1047,13 +1048,13 @@ bool Logger::get_log_time(struct tm *tt, bool boot_time) return gmtime_r(&utc_time_sec, tt) != nullptr; } -void Logger::start_log() +void Logger::start_log_file() { - if (_writer.is_started()) { + if (_writer.is_started_file() || (_writer.backend() & LogWriter::BackendFile) == 0) { return; } - PX4_INFO("start log"); + PX4_INFO("start file log"); char file_name[LOG_DIR_LEN] = ""; @@ -1066,7 +1067,7 @@ void Logger::start_log() mavlink_log_info(&_mavlink_log_pub, "[logger] file: %s", file_name); _next_topic_id = 0; - _writer.start_log(file_name); + _writer.start_log_file(file_name); write_header(); write_version(); write_formats(); @@ -1076,13 +1077,9 @@ void Logger::start_log() _start_time = hrt_absolute_time(); } -void Logger::stop_log() +void Logger::stop_log_file() { - if (!_writer.is_started()) { - return; - } - - _writer.stop_log(); + _writer.stop_log_file(); } bool Logger::write_wait(void *ptr, size_t size) diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 4c50f90ab0..6eb747229b 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -81,7 +81,7 @@ struct LoggerSubscription { class Logger { public: - Logger(size_t buffer_size, uint32_t log_interval, bool log_on_start, + Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, bool log_on_start, bool log_until_shutdown, bool log_name_timestamp); ~Logger(); @@ -152,9 +152,9 @@ private: */ int check_free_space(); - void start_log(); + void start_log_file(); - void stop_log(); + void stop_log_file(); /** * write the file header with file magic and timestamp.