From 4e0129275d6e0e474b19742375bf3042e038ee6f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 25 Apr 2016 17:36:13 -0400 Subject: [PATCH] new logger --- ROMFS/px4fmu_common/init.d/rc.logging | 40 + ROMFS/px4fmu_common/init.d/rcS | 31 +- Tools/check_code_style_all.sh | 1 + cmake/configs/nuttx_mindpx-v2_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v1_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v2_ekf2.cmake | 1 + cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + cmake/configs/posix_eagle_hil.cmake | 1 + .../posix_eagle_legacy_driver_default.cmake | 1 + cmake/configs/posix_rpi2_default.cmake | 1 + cmake/configs/posix_rpi2_release.cmake | 1 + cmake/configs/posix_sdflight_default.cmake | 1 + cmake/configs/posix_sitl_broadcast.cmake | 1 + cmake/configs/posix_sitl_default.cmake | 2 +- cmake/configs/posix_sitl_ekf2.cmake | 1 + cmake/configs/posix_sitl_replay.cmake | 1 + cmake/configs/posix_sitl_test.cmake | 1 + cmake/ros-CMakeLists.txt | 37 +- src/modules/logger/CMakeLists.txt | 52 ++ src/modules/logger/array.h | 127 +++ src/modules/logger/log_writer.cpp | 237 ++++++ src/modules/logger/log_writer.h | 74 ++ src/modules/logger/logger.cpp | 760 ++++++++++++++++++ src/modules/logger/logger.h | 96 +++ src/modules/systemlib/system_params.c | 12 + 26 files changed, 1437 insertions(+), 46 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.logging create mode 100644 src/modules/logger/CMakeLists.txt create mode 100644 src/modules/logger/array.h create mode 100644 src/modules/logger/log_writer.cpp create mode 100644 src/modules/logger/log_writer.h create mode 100644 src/modules/logger/logger.cpp create mode 100644 src/modules/logger/logger.h diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging new file mode 100644 index 0000000000..f46d94dbc7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -0,0 +1,40 @@ +#!nsh +# +# Initialize logging services. +# + +if [ -d /fs/microsd ] +then + if ver hwcmp PX4FMU_V1 + then + if sdlog2 start -r 30 -a -b 2 -t + then + fi + else + # check if we should increase logging rate for ekf2 replay message logging + if param greater EKF2_REC_RPL 0 + then + if param compare SYS_LOGGER 0 + then + if sdlog2 start -r 500 -e -b 20 -t + then + fi + else + if logger start -r 500 + then + fi + fi + else + if param compare SYS_LOGGER 0 + then + if sdlog2 start -r 100 -a -b 12 -t + then + fi + else + if logger start -b 32 + then + fi + fi + fi + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1e2bc91bb9..cfcdfa5397 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -617,32 +617,6 @@ then mavlink start -r 800000 -d /dev/ttyACM0 -m config -x fi - - # - # Logging - # - if [ -d /fs/microsd ] - then - if ver hwcmp PX4FMU_V1 - then - if sdlog2 start -r 30 -a -b 2 -t - then - fi - else - # check if we should increase logging rate for ekf2 replay message logging - if param greater EKF2_REC_RPL 0 - then - if sdlog2 start -r 500 -e -b 20 -t - then - fi - else - if sdlog2 start -r 100 -a -b 12 -t - then - fi - fi - fi - fi - # # Start up ARDrone Motor interface # @@ -863,6 +837,11 @@ then echo "[i] No autostart ID found" fi + # + # Logging + # + sh /etc/init.d/rc.logging + # Start any custom addons set FEXTRAS /fs/microsd/etc/extras.txt if [ -f $FEXTRAS ] diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh index eba8648f25..3dde2105dd 100755 --- a/Tools/check_code_style_all.sh +++ b/Tools/check_code_style_all.sh @@ -26,6 +26,7 @@ find \ src/modules/gpio_led \ src/modules/land_detector \ src/modules/local_position_estimator \ + src/modules/logger \ src/modules/mavlink/mavlink_tests \ src/modules/muorb \ src/modules/param \ diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake index 9f0626e5b7..e09339029f 100644 --- a/cmake/configs/nuttx_mindpx-v2_default.cmake +++ b/cmake/configs/nuttx_mindpx-v2_default.cmake @@ -105,6 +105,7 @@ set(config_module_list # Logging # modules/sdlog2 + modules/logger # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index dfb672a5a8..75f8d054ed 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -91,6 +91,7 @@ set(config_module_list # Logging # modules/sdlog2 + modules/logger # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index a30cd9b381..ee1263bad6 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -100,6 +100,7 @@ set(config_module_list # # Logging # + modules/logger modules/sdlog2 # diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 33daab5f77..289eb4098a 100644 --- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -99,6 +99,7 @@ set(config_module_list # Logging # modules/sdlog2 + modules/logger # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index eb4713eb50..8a3d8f39f3 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -104,6 +104,7 @@ set(config_module_list # Logging # modules/sdlog2 + modules/logger # # Library modules diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake index 3e6274956f..63ca567be7 100644 --- a/cmake/configs/posix_eagle_hil.cmake +++ b/cmake/configs/posix_eagle_hil.cmake @@ -27,6 +27,7 @@ set(config_module_list modules/sensors modules/dataman modules/sdlog2 + modules/logger modules/simulator modules/commander modules/load_mon diff --git a/cmake/configs/posix_eagle_legacy_driver_default.cmake b/cmake/configs/posix_eagle_legacy_driver_default.cmake index 8b82f52613..ed6cd83c16 100644 --- a/cmake/configs/posix_eagle_legacy_driver_default.cmake +++ b/cmake/configs/posix_eagle_legacy_driver_default.cmake @@ -45,6 +45,7 @@ set(config_module_list modules/sensors modules/dataman modules/sdlog2 + modules/logger modules/simulator modules/commander diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_default.cmake index c67d241f9f..ca677261be 100644 --- a/cmake/configs/posix_rpi2_default.cmake +++ b/cmake/configs/posix_rpi2_default.cmake @@ -31,6 +31,7 @@ set(config_module_list modules/fw_pos_control_l1 modules/dataman modules/sdlog2 + modules/logger modules/commander modules/load_mon lib/controllib diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake index e4272e6ef4..b10914951a 100644 --- a/cmake/configs/posix_rpi2_release.cmake +++ b/cmake/configs/posix_rpi2_release.cmake @@ -40,6 +40,7 @@ set(config_module_list modules/fw_pos_control_l1 modules/dataman modules/sdlog2 + modules/logger modules/commander modules/load_mon lib/controllib diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake index ef1aef2e2b..b6c686c9d4 100644 --- a/cmake/configs/posix_sdflight_default.cmake +++ b/cmake/configs/posix_sdflight_default.cmake @@ -38,6 +38,7 @@ set(config_module_list modules/sensors modules/dataman modules/sdlog2 + modules/logger modules/simulator modules/commander modules/navigator diff --git a/cmake/configs/posix_sitl_broadcast.cmake b/cmake/configs/posix_sitl_broadcast.cmake index 2e92a2f08d..9a9029f1c0 100644 --- a/cmake/configs/posix_sitl_broadcast.cmake +++ b/cmake/configs/posix_sitl_broadcast.cmake @@ -49,6 +49,7 @@ set(config_module_list modules/fw_pos_control_l1 modules/dataman modules/sdlog2 + modules/logger modules/commander lib/controllib lib/mathlib diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 5d3f364be8..e97d62e3a2 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -40,6 +40,7 @@ set(config_module_list modules/fw_pos_control_l1 modules/land_detector modules/load_mon + modules/logger modules/mavlink modules/mc_att_control modules/mc_att_control_multiplatform @@ -56,7 +57,6 @@ set(config_module_list modules/systemlib/mixer modules/uORB modules/vtol_att_control - lib/controllib lib/conversion lib/DriverFramework/framework diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake index 7da1e16d4a..d92ed793a0 100644 --- a/cmake/configs/posix_sitl_ekf2.cmake +++ b/cmake/configs/posix_sitl_ekf2.cmake @@ -48,6 +48,7 @@ set(config_module_list modules/fw_pos_control_l1 modules/dataman modules/sdlog2 + modules/logger modules/commander modules/load_mon lib/controllib diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake index 959e389bdb..3c99cba7a7 100644 --- a/cmake/configs/posix_sitl_replay.cmake +++ b/cmake/configs/posix_sitl_replay.cmake @@ -17,6 +17,7 @@ set(config_module_list modules/ekf2 modules/ekf2_replay modules/sdlog2 + modules/logger lib/controllib lib/mathlib lib/mathlib/math/filter diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake index 5a7d9cd9bd..22e59e9425 100644 --- a/cmake/configs/posix_sitl_test.cmake +++ b/cmake/configs/posix_sitl_test.cmake @@ -40,6 +40,7 @@ set(config_module_list modules/fw_pos_control_l1 modules/land_detector modules/load_mon + modules/logger modules/mavlink modules/mc_att_control modules/mc_att_control_multiplatform diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt index a6df11a3f6..10cb673bfe 100644 --- a/cmake/ros-CMakeLists.txt +++ b/cmake/ros-CMakeLists.txt @@ -59,30 +59,29 @@ find_package(Eigen REQUIRED) ## Generate messages in the 'msg' folder add_message_files( FILES - rc_channels.msg - vehicle_attitude.msg - vehicle_attitude_setpoint.msg - manual_control_setpoint.msg - actuator_controls.msg - actuator_controls_0.msg - actuator_controls_virtual_mc.msg - vehicle_rates_setpoint.msg - mc_virtual_rates_setpoint.msg - vehicle_attitude.msg - vehicle_control_mode.msg actuator_armed.msg - parameter_update.msg - vehicle_status.msg + actuator_controls.msg commander_state.msg - vehicle_local_position.msg + control_state.msg + distance_sensor.msg + manual_control_setpoint.msg + mc_virtual_rates_setpoint.msg + offboard_control_mode.msg + parameter_update.msg position_setpoint.msg position_setpoint_triplet.msg - vehicle_local_position_setpoint.msg - vehicle_global_velocity_setpoint.msg - offboard_control_mode.msg + rc_channels.msg + ros/actuator_controls_0.msg + ros/actuator_controls_virtual_mc.msg + vehicle_attitude.msg + vehicle_attitude_setpoint.msg + vehicle_control_mode.msg vehicle_force_setpoint.msg - distance_sensor.msg - control_state.msg + vehicle_global_velocity_setpoint.msg + vehicle_local_position.msg + vehicle_local_position_setpoint.msg + vehicle_rates_setpoint.msg + vehicle_status.msg ) ## Generate services in the 'srv' folder diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt new file mode 100644 index 0000000000..577f9f1be0 --- /dev/null +++ b/src/modules/logger/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ +if (${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=2200) +endif() + +px4_add_module( + MODULE modules__logger + MAIN logger + PRIORITY "SCHED_PRIORITY_MAX-30" + STACK_MAIN 2200 + COMPILE_FLAGS + ${MODULE_CFLAGS} + -Os + SRCS + logger.cpp + log_writer.cpp + DEPENDS + platforms__common + modules__uORB + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/logger/array.h b/src/modules/logger/array.h new file mode 100644 index 0000000000..78862ff072 --- /dev/null +++ b/src/modules/logger/array.h @@ -0,0 +1,127 @@ +#pragma once + +#include + +namespace px4 +{ + +template +class Array +{ + typedef TYPE &reference; + typedef const TYPE &const_reference; + typedef TYPE *iterator; + typedef const TYPE *const_iterator; + +public: + Array() + : _size(0), _overflow(false) + {} + + bool push_back(const TYPE &x) + { + if (_size == N) { + _overflow = true; + return false; + + } else { + _items[_size] = x; + ++_size; + return true; + } + } + + void remove(unsigned idx) + { + if (idx < _size) { + --_size; + + for (unsigned i = idx; i < _size; ++i) { + _items[i] = _items[i + 1]; + } + } + } + + reference operator[](size_t n) + { + return _items[n]; + } + + const_reference operator[](size_t n) const + { + return _items[n]; + } + + reference at(size_t n) + { + return _items[n]; + } + + const_reference at(size_t n) const + { + return _items[n]; + } + + size_t size() const + { + return _size; + } + + size_t max_size() const + { + return N; + } + + size_t capacity() const + { + return N; + } + + bool empty() const + { + return _size == 0; + } + + bool is_overflowed() + { + return _overflow; + } + + iterator begin() + { + return &_items[0]; + } + + iterator end() + { + return &_items[_size]; + } + + const_iterator begin() const + { + return &_items[0]; + } + + const_iterator end() const + { + return &_items[_size]; + } + + void erase(iterator item) + { + if (item - _items < static_cast(_size)) { + --_size; + + for (iterator it = item; it != &_items[_size]; ++it) { + *it = *(it + 1); + } + } + } + +private: + TYPE _items[N]; + size_t _size; + bool _overflow; +}; + +} diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp new file mode 100644 index 0000000000..249180167a --- /dev/null +++ b/src/modules/logger/log_writer.cpp @@ -0,0 +1,237 @@ +#include "log_writer.h" +#include +#include + +namespace px4 +{ +namespace logger +{ + +LogWriter::LogWriter(uint8_t *buffer, size_t buffer_size) : + _buffer(buffer), + _buffer_size(buffer_size), + _min_write_chunk(4096) +{ + 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"); +} + +void LogWriter::start_log(const char *filename) +{ + ::strncpy(_filename, filename, sizeof(_filename)); + // Clear buffer and counters + _head = 0; + _count = 0; + _total_written = 0; + _should_run = true; + notify(); +} + +void LogWriter::stop_log() +{ + _should_run = false; + notify(); +} + +pthread_t LogWriter::thread_start() +{ + 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, 1024); + + pthread_t thr; + + if (0 != pthread_create(&thr, &thr_attr, &LogWriter::run_helper, this)) { + PX4_WARN("error creating logwriter thread"); + } + + pthread_attr_destroy(&thr_attr); + + return thr; +} + +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() +{ + // 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; + } + } + + while (!_exit_thread) { + // Outer endless loop, start new file each time + // _filename must be set before setting _should_run = true + + + _fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666); + + if (_fd < 0) { + PX4_WARN("can't open log file %s", _filename); + _should_run = false; + continue; + } + + PX4_WARN("started, log file: %s", _filename); + + _should_run = true; + + 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); + + 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 + break; + } + } + + _head = 0; + _count = 0; + + int res = ::close(_fd); + + if (res) { + PX4_WARN("error closing log file"); + } + + PX4_WARN("stopped, bytes written: %zu", _total_written); + } +} + +bool LogWriter::write(void *ptr, size_t size) +{ + + // Bytes available to write + size_t available = _buffer_size - _count; + + if (size > available) { + // buffer overflow + return false; + } + + 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; + return true; +} + +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 new file mode 100644 index 0000000000..f6fd35c4de --- /dev/null +++ b/src/modules/logger/log_writer.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace px4 +{ +namespace logger +{ + +class LogWriter +{ + friend class Logger; +public: + LogWriter(uint8_t *buffer, size_t buffer_size); + + pthread_t thread_start(); + + void thread_stop(); + + void start_log(const char *filename); + + void stop_log(); + + bool write(void *ptr, size_t size); + + void lock() + { + pthread_mutex_lock(&_mtx); + } + + void unlock() + { + pthread_mutex_unlock(&_mtx); + } + + void notify() + { + pthread_cond_broadcast(&_cv); + } + +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; + } + + char _filename[64]; + int _fd; + uint8_t *_buffer; + const size_t _buffer_size; + const size_t _min_write_chunk; /* 512 didn't seem to work properly, 4096 should match the FAT cluster size */ + size_t _head = 0; + size_t _count = 0; + size_t _total_written = 0; + bool _should_run = 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 new file mode 100644 index 0000000000..3005690ce2 --- /dev/null +++ b/src/modules/logger/logger.cpp @@ -0,0 +1,760 @@ +#include "logger.h" + +#include +#include +#include + +#include +#include + +#define DBGPRINT + +#if defined(DBGPRINT) +// needed for mallinfo +#if defined(__PX4_POSIX) && !defined(__PX4_DARWIN) +#include +#endif /* __PX4_POSIX */ + +// struct mallinfo not available on OSX? +#if defined(__PX4_DARWIN) +#undef DBGPRINT +#endif /* defined(__PX4_DARWIN) */ +#endif /* defined(DBGPRINT) */ + +using namespace px4::logger; + +int logger_main(int argc, char *argv[]) +{ + if (argc < 2) { + PX4_INFO("usage: logger {start|stop|status}"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (logger_ptr != nullptr) { + PX4_INFO("already running"); + return 1; + } + + if (OK != Logger::start((char *const *)argv)) { + PX4_WARN("start failed"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (logger_ptr == nullptr) { + PX4_INFO("not running"); + return 1; + } + + delete logger_ptr; + logger_ptr = nullptr; + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (logger_ptr) { + logger_ptr->status(); + return 0; + + } else { + PX4_INFO("not running"); + return 1; + } + } + + Logger::usage("unrecognized command"); + return 1; +} + +namespace px4 +{ +namespace logger +{ + +void Logger::usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PX4_INFO("usage: logger {start|stop|status} [-r ] [-b ] -e -a -t -x\n" + "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-b\tLog buffer size in KiB, default is 8\n" + "\t-e\tEnable logging by default (if not, can be started by command)\n" + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n" + "\t-x\tExtended logging"); +} + +int Logger::start(char *const *argv) +{ + ASSERT(logger_task == -1); + + /* start the task */ + logger_task = px4_task_spawn_cmd("logger", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3100, + (px4_main_t)&Logger::run_trampoline, + (char *const *)argv); + + if (logger_task < 0) { + PX4_WARN("task start failed"); + return -errno; + } + + return OK; +} + +void Logger::status() +{ + if (!_enabled) { + PX4_INFO("running, but not logging"); + + } else { + PX4_INFO("running"); + +// float kibibytes = _writer.get_total_written() / 1024.0f; +// float mebibytes = kibibytes / 1024.0f; +// float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f; +// +// PX4_WARN("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); +// mavlink_log_info(&mavlink_log_pub, "[blackbox] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + } +} + +void Logger::run_trampoline(int argc, char *argv[]) +{ + unsigned log_interval = 3500; + int log_buffer_size = 12 * 1024; + bool log_on_start = false; + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "r:b:eatx", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'r': { + unsigned long r = strtoul(myoptarg, NULL, 10); + + if (r <= 0) { + r = 1; + } + + log_interval = 1e6 / r; + } + break; + + case 'e': + log_on_start = true; + break; + + case 'b': { + unsigned long s = strtoul(myoptarg, NULL, 10); + + if (s < 1) { + s = 1; + } + + log_buffer_size = 1024 * s; + } + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + logger_task = -1; + return; + } + + logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start); + + if (logger_ptr == nullptr) { + PX4_WARN("alloc failed"); + + } else { + logger_ptr->run(); + } +} + +enum class MessageType : uint8_t { + FORMAT = 'F', + DATA = 'D', + INFO = 'I', + PARAMETER = 'P', +}; + +/* declare message data structs with byte alignment (no padding) */ +#pragma pack(push, 1) +struct message_format_s { + uint8_t msg_type = static_cast(MessageType::FORMAT); + uint16_t msg_size; + + uint8_t msg_id; + uint16_t format_len; + char format[2096]; +}; + +struct message_data_header_s { + uint8_t msg_type = static_cast(MessageType::DATA); + uint16_t msg_size; + + uint8_t msg_id; + uint8_t multi_id; +}; + +// currently unused +struct message_info_header_s { + uint8_t msg_type = static_cast(MessageType::INFO); + uint16_t msg_size; + + uint8_t key_len; + char key[255]; +}; + +struct message_parameter_header_s { + uint8_t msg_type = static_cast(MessageType::PARAMETER); + uint16_t msg_size; + + uint8_t key_len; + char key[255]; +}; +#pragma pack(pop) + + +static constexpr size_t MAX_DATA_SIZE = 740; + +Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start) : + _log_on_start(log_on_start), + _writer((_log_buffer = new uint8_t[buffer_size]), buffer_size), + _log_interval(log_interval) +{ +} + +Logger::~Logger() +{ + if (logger_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned int i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 200) { + px4_task_delete(logger_task); + logger_task = -1; + break; + } + } while (logger_task != -1); + } + + delete [] _log_buffer; + logger_ptr = nullptr; +} + +int Logger::add_topic(const orb_metadata *topic) +{ + int fd = -1; + + if (topic->o_size > MAX_DATA_SIZE) { + PX4_WARN("skip topic %s, data size is too large: %zu (max is %zu)", topic->o_name, topic->o_size, MAX_DATA_SIZE); + + } else { + size_t fields_len = strlen(topic->o_fields); + + if (fields_len > sizeof(message_format_s::format)) { + PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len, + sizeof(message_format_s::format)); + + } else { + fd = orb_subscribe(topic); + _subscriptions.push_back(LoggerSubscription(fd, topic)); + } + } + + return fd; +} + +int Logger::add_topic(const char *name, unsigned interval = 0) +{ + const orb_metadata **topics = orb_get_topics(); + int fd = -1; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(name, topics[i]->o_name) == 0) { + fd = add_topic(topics[i]); + printf("logging topic: %zu, %s\n", i, topics[i]->o_name); + break; + } + } + + if ((fd > 0) && (interval != 0)) { + orb_set_interval(fd, interval); + } + + return fd; +} + +bool Logger::copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, + uint64_t *time_last_checked) +{ + bool updated = false; + + // only try to subscribe to topic if this is the first time + // after that just check after a certain interval to avoid high cpu usage + if (*handle < 0 && (*time_last_checked == 0 || hrt_elapsed_time(time_last_checked) > TRY_SUBSCRIBE_INTERVAL)) { + //if (multi_instance == 1) warnx("checking instance 1 of topic %s", topic->o_name); + *time_last_checked = hrt_absolute_time(); + + if (OK == orb_exists(topic, multi_instance)) { + *handle = orb_subscribe_multi(topic, multi_instance); + + //warnx("subscribed to instance %d of topic %s", multi_instance, topic->o_name); + + /* copy first data */ + if (*handle >= 0) { + orb_copy(topic, *handle, buffer); + updated = true; + } + } + + } else if (*handle >= 0) { + orb_check(*handle, &updated); + + if (updated) { + orb_copy(topic, *handle, buffer); + } + } + + return updated; +} + +void Logger::run() +{ +#ifdef DBGPRINT + struct mallinfo alloc_info = {}; +#endif /* DBGPRINT */ + + PX4_WARN("started"); + + int mkdir_ret = mkdir(LOG_ROOT, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + PX4_WARN("log root dir created: %s", LOG_ROOT); + + } else if (errno != EEXIST) { + PX4_WARN("failed creating log root dir: %s", LOG_ROOT); + return; + } + + add_topic("manual_control_setpoint", 10); + add_topic("vehicle_rates_setpoint", 10); + add_topic("vehicle_attitude_setpoint", 10); + add_topic("vehicle_attitude", 10); + add_topic("actuator_outputs", 50); + add_topic("battery_status", 100); + add_topic("vehicle_command", 100); + add_topic("actuator_controls", 10); + add_topic("vehicle_local_position_setpoint", 30); + add_topic("rc_channels", 100); + add_topic("ekf2_innovations", 20); + add_topic("commander_state", 100); + add_topic("vehicle_local_position", 10); + add_topic("vehicle_global_position", 10); + add_topic("system_power", 100); + add_topic("servorail_status", 100); + add_topic("mc_att_ctrl_status", 50); + add_topic("control_state"); + add_topic("estimator_status"); + add_topic("vehicle_status", 20); + + _writer_thread = _writer.thread_start(); + + _task_should_exit = false; + +#ifdef DBGPRINT + hrt_abstime dropout_start = 0; + hrt_abstime timer_start = 0; + uint32_t total_bytes = 0; + uint16_t dropout_count = 0; + size_t highWater = 0; + size_t available = 0; + double max_drop_len = 0; +#endif /* DBGPRINT */ + + // we start logging immediately + // the case where we wait with logging until vehicle is armed is handled below + if (_log_on_start) { + start_log(); + } + + /* every log_interval usec, check for orb updates */ + while (!_task_should_exit) { + + // Start/stop logging when system arm/disarm + if (_vehicle_status_sub.check_updated()) { + _vehicle_status_sub.update(); + bool armed = (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) || + (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); + + if (_enabled != armed && !_log_on_start) { + if (armed) { + start_log(); + +#ifdef DBGPRINT + timer_start = hrt_absolute_time(); + total_bytes = 0; +#endif /* DBGPRINT */ + + } else { + stop_log(); + } + } + } + + if (_enabled) { + /* wait for lock on log buffer */ + _writer.lock(); + + bool data_written = false; + + // Write data messages for normal subscriptions + int msg_id = 0; + + for (LoggerSubscription &sub : _subscriptions) { + /* each message consists of a header followed by an orb data object + * The size of the data object is given by orb_metadata.o_size + */ + size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size; + uint8_t buffer[msg_size]; + + /* if this topic has been updated, copy the new data into the message buffer + * and write a message to the log + */ + //orb_check(sub.fd, &updated); // check whether a non-multi topic has been updated + /* this works for both single and multi-instances */ + for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { + if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s), + &sub.time_tried_subscribe)) { + + //uint64_t timestamp; + //memcpy(×tamp, buffer + sizeof(message_data_header_s), sizeof(timestamp)); + //warnx("topic: %s, instance: %d, timestamp: %llu", + // sub.metadata->o_name, instance, timestamp); + + /* copy the current topic data into the buffer after the header */ + //orb_copy(sub.metadata, sub.fd, buffer + sizeof(message_data_header_s)); + + /* fill the message header struct in-place at the front of the buffer, + * accessing the unaligned (packed) structure properly + */ + message_data_header_s *header = reinterpret_cast(buffer); + header->msg_type = static_cast(MessageType::DATA); + /* the ORB topic data object has 2 unused trailing bytes? */ + header->msg_size = static_cast(msg_size - 2); + header->msg_id = msg_id; + header->multi_id = 0x80 + instance; // Non multi, active + +#ifdef DBGPRINT + //warnx("subscription %s updated: %d, size: %d", sub.metadata->o_name, updated, msg_size); + hrt_abstime trytime = hrt_absolute_time(); + + if (_writer._count > highWater) { + highWater = _writer._count; + } + +#endif /* DBGPRINT */ + + if (_writer.write(buffer, msg_size)) { + +#ifdef DBGPRINT + + // successful write: note end of dropout if dropout_start != 0 + if (dropout_start != 0) { + double drop_len = (double)(trytime - dropout_start) * 1e-6; + + if (drop_len > max_drop_len) { max_drop_len = drop_len; } + + PX4_WARN("dropout length: %5.3f seconds", drop_len); + dropout_start = 0; + highWater = 0; + } + + total_bytes += msg_size; +#endif /* DBGPRINT */ + + data_written = true; + + } else { + +#ifdef DBGPRINT + + if (dropout_start == 0) { + available = _writer._count; + PX4_WARN("dropout, available: %d/%d", available, _writer._buffer_size); + dropout_start = trytime; + dropout_count++; + } + +#endif /* DBGPRINT */ + + break; // Write buffer overflow, skip this record + } + } + } + + msg_id++; + } + + /* release the log buffer */ + _writer.unlock(); + + /* notify the writer thread if data is available */ + if (data_written) { + _writer.notify(); + } + +#ifdef DBGPRINT + double deltat = (double)(hrt_absolute_time() - timer_start) * 1e-6; + + if (deltat > 4.0) { + alloc_info = mallinfo(); + double throughput = total_bytes / deltat; + PX4_INFO("%8.1e Kbytes/sec, %d highWater, %d dropouts, %5.3f sec max, free heap: %d", + throughput / 1e3, highWater, dropout_count, max_drop_len, alloc_info.fordblks); + + total_bytes = 0; + highWater = 0, + dropout_count = 0; + max_drop_len = 0; + timer_start = hrt_absolute_time(); + } + +#endif /* DBGPRINT */ + + } + + usleep(_log_interval); + } + + // stop the writer thread + _writer.thread_stop(); + + _writer.notify(); + + // wait for thread to complete + int ret = pthread_join(_writer_thread, NULL); + + if (ret) { + PX4_WARN("join failed: %d", ret); + } + + logger_task = -1; +} + +int Logger::create_log_dir() +{ + /* create dir on sdcard if needed */ + uint16_t dir_number = 1; // start with dir sess001 + int mkdir_ret; + + /* look for the next dir that does not exist */ + while (dir_number <= MAX_NO_LOGFOLDER) { + /* format log dir: e.g. /fs/microsd/sess001 */ + sprintf(_log_dir, "%s/sess%03u", LOG_ROOT, dir_number); + mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + PX4_INFO("log dir created: %s", _log_dir); + break; + + } else if (errno != EEXIST) { + PX4_WARN("failed creating new dir: %s", _log_dir); + return -1; + } + + /* dir exists already */ + dir_number++; + continue; + } + + if (dir_number >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + PX4_WARN("all %d possible dirs exist already", MAX_NO_LOGFOLDER); + return -1; + } + + /* print logging path, important to find log file later */ + //mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + return 0; +} + +bool Logger::file_exist(const char *filename) +{ + struct stat buffer; + return stat(filename, &buffer) == 0; +} + +int Logger::get_log_file_name(char *file_name, size_t file_name_size) +{ + uint16_t file_number = 1; // start with file log001 + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.ulg */ + snprintf(file_name, file_name_size, "%s/log%03u.ulg", _log_dir, file_number); + + if (!file_exist(file_name)) { + break; + } + + file_number++; + } + + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + //mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + return -1; + } + + return 0; +} + +void Logger::start_log() +{ + PX4_WARN("start log"); + + if (create_log_dir()) { + return; + } + + char file_name[64] = ""; + + if (get_log_file_name(file_name, sizeof(file_name))) { + return; + } + + _writer.start_log(file_name); + write_formats(); + write_parameters(); + _enabled = true; +} + +void Logger::stop_log() +{ + _enabled = false; + _writer.stop_log(); +} + +void Logger::write_formats() +{ + _writer.lock(); + message_format_s msg; + + int msg_id = 0; + + for (LoggerSubscription &sub : _subscriptions) { + msg.msg_id = msg_id; + msg.format_len = snprintf(msg.format, sizeof(msg.format), "%s", sub.metadata->o_fields); + size_t msg_size = sizeof(msg) - sizeof(msg.format) + msg.format_len; + msg.msg_size = msg_size - 2; + + while (!_writer.write(&msg, msg_size)) { + _writer.unlock(); + _writer.notify(); + usleep(_log_interval); // Wait if buffer is full, don't skip FORMAT messages + _writer.lock(); + } + + msg_id++; + } + + _writer.unlock(); + _writer.notify(); +} + +void Logger::write_parameters() +{ + _writer.lock(); + uint8_t buffer[sizeof(message_parameter_header_s) + sizeof(param_value_u)]; + message_parameter_header_s *msg = reinterpret_cast(buffer); + + msg->msg_type = static_cast(MessageType::PARAMETER); + int param_idx = 0; + param_t param = 0; + + do { + do { + param = param_for_index(param_idx); + ++param_idx; + } while (param != PARAM_INVALID && !param_used(param)); + + if (param != PARAM_INVALID) { + /* get parameter type and size */ + const char *type_str; + param_type_t type = param_type(param); + size_t value_size = 0; + + switch (type) { + case PARAM_TYPE_INT32: + type_str = "int32_t"; + value_size = sizeof(int32_t); + break; + + case PARAM_TYPE_FLOAT: + type_str = "float"; + value_size = sizeof(float); + break; + + default: + continue; + } + + /* format parameter key (type and name) */ + msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s", type_str, param_name(param)); + size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len; + + /* copy parameter value directly to buffer */ + param_get(param, &buffer[msg_size]); + msg_size += value_size; + + msg->msg_size = msg_size - 2; + + /* write message */ + while (!_writer.write(buffer, msg_size)) { + /* wait if buffer is full, don't skip PARAMETER messages */ + _writer.unlock(); + _writer.notify(); + usleep(_log_interval); + _writer.lock(); + } + } + } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); + + _writer.unlock(); + _writer.notify(); +} + +} +} diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h new file mode 100644 index 0000000000..f7a30c6f0f --- /dev/null +++ b/src/modules/logger/logger.h @@ -0,0 +1,96 @@ +#pragma once + +#include "log_writer.h" +#include "array.h" +#include +#include +#include +#include + +extern "C" __EXPORT int logger_main(int argc, char *argv[]); + +#define TRY_SUBSCRIBE_INTERVAL 1000*1000 // interval in microseconds at which we try to subscribe to a topic +// if we haven't succeeded before + +namespace px4 +{ +namespace logger +{ + +struct LoggerSubscription { + int fd[ORB_MULTI_MAX_INSTANCES]; + uint64_t time_tried_subscribe; // captures the time at which we checked last time if this instance existed + const orb_metadata *metadata = nullptr; + + LoggerSubscription() {} + + LoggerSubscription(int fd_, const orb_metadata *metadata_) : + metadata(metadata_) + { + fd[0] = fd_; + time_tried_subscribe = 0; + + for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { fd[i] = -1; } + } +}; + +class Logger +{ +public: + Logger(size_t buffer_size, unsigned log_interval, bool log_on_start); + + ~Logger(); + + int add_topic(const orb_metadata *topic); + + int add_topic(const char *name, unsigned interval); + + static int start(char *const *argv); + + static void usage(const char *reason); + + void status(); + +private: + static void run_trampoline(int argc, char *argv[]); + + void run(); + + int create_log_dir(); + + static bool file_exist(const char *filename); + + int get_log_file_name(char *file_name, size_t file_name_size); + + void start_log(); + + void stop_log(); + + void write_formats(); + + void write_parameters(); + + bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, uint64_t *time_last_checked); + + static constexpr size_t MAX_TOPICS_NUM = 128; + static constexpr unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ + static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ + static constexpr const char *LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log"; + + bool _task_should_exit = true; + uint8_t *_log_buffer; + char _log_dir[64]; + uORB::Subscription _vehicle_status_sub {ORB_ID(vehicle_status)}; + bool _enabled = false; + bool _log_on_start; + Array _subscriptions; + LogWriter _writer; + uint32_t _log_interval; +}; + +Logger *logger_ptr; +int logger_task = -1; +pthread_t _writer_thread; + +} +} diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index dae66295aa..692f919126 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -139,3 +139,15 @@ PARAM_DEFINE_INT32(SYS_COMPANION, 157600); * @group System */ PARAM_DEFINE_INT32(SYS_PARAM_VER, 1); + +/** + * SD logger + * + * @value 0 sdlog2 (default) + * @value 1 new logger (experimental) + * @min 0 + * @max 1 + * @reboot_required true + * @group System + */ +PARAM_DEFINE_INT32(SYS_LOGGER, 0);