From 4e0129275d6e0e474b19742375bf3042e038ee6f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
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 | 35 +-
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, 1436 insertions(+), 45 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_armed.msg
actuator_controls.msg
- actuator_controls_0.msg
- actuator_controls_virtual_mc.msg
- vehicle_rates_setpoint.msg
+ commander_state.msg
+ control_state.msg
+ distance_sensor.msg
+ manual_control_setpoint.msg
mc_virtual_rates_setpoint.msg
- vehicle_attitude.msg
- vehicle_control_mode.msg
- actuator_armed.msg
+ offboard_control_mode.msg
parameter_update.msg
- vehicle_status.msg
- commander_state.msg
- vehicle_local_position.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 <stdlib.h>
+
+namespace px4
+{
+
+template <typename TYPE, size_t N>
+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<int>(_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 <fcntl.h>
+#include <string.h>
+
+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<LogWriter *>(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<int>(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<uint8_t *>(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 <px4.h>
+#include <stdint.h>
+#include <pthread.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/perf_counter.h>
+
+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 <sys/stat.h>
+#include <errno.h>
+#include <string.h>
+
+#include <uORB/uORBTopics.h>
+#include <px4_getopt.h>
+
+#define DBGPRINT
+
+#if defined(DBGPRINT)
+// needed for mallinfo
+#if defined(__PX4_POSIX) && !defined(__PX4_DARWIN)
+#include <malloc.h>
+#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 <log rate>] [-b <buffer size>] -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<uint8_t>(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<uint8_t>(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<uint8_t>(MessageType::INFO);
+ uint16_t msg_size;
+
+ uint8_t key_len;
+ char key[255];
+};
+
+struct message_parameter_header_s {
+ uint8_t msg_type = static_cast<uint8_t>(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<message_data_header_s *>(buffer);
+ header->msg_type = static_cast<uint8_t>(MessageType::DATA);
+ /* the ORB topic data object has 2 unused trailing bytes? */
+ header->msg_size = static_cast<uint16_t>(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<message_parameter_header_s *>(buffer);
+
+ msg->msg_type = static_cast<uint8_t>(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 <px4.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
+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_s> _vehicle_status_sub {ORB_ID(vehicle_status)};
+ bool _enabled = false;
+ bool _log_on_start;
+ Array<LoggerSubscription, MAX_TOPICS_NUM> _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);
--
GitLab