new logger

This commit is contained in:
Daniel Agar
2016-04-25 17:36:13 -04:00
committed by Lorenz Meier
parent eb29b33620
commit 4e0129275d
26 changed files with 1437 additions and 46 deletions
+40
View File
@@ -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
+5 -26
View File
@@ -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 ]
+1
View File
@@ -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 \
@@ -105,6 +105,7 @@ set(config_module_list
# Logging
#
modules/sdlog2
modules/logger
#
# Library modules
@@ -91,6 +91,7 @@ set(config_module_list
# Logging
#
modules/sdlog2
modules/logger
#
# Library modules
@@ -100,6 +100,7 @@ set(config_module_list
#
# Logging
#
modules/logger
modules/sdlog2
#
+1
View File
@@ -99,6 +99,7 @@ set(config_module_list
# Logging
#
modules/sdlog2
modules/logger
#
# Library modules
@@ -104,6 +104,7 @@ set(config_module_list
# Logging
#
modules/sdlog2
modules/logger
#
# Library modules
+1
View File
@@ -27,6 +27,7 @@ set(config_module_list
modules/sensors
modules/dataman
modules/sdlog2
modules/logger
modules/simulator
modules/commander
modules/load_mon
@@ -45,6 +45,7 @@ set(config_module_list
modules/sensors
modules/dataman
modules/sdlog2
modules/logger
modules/simulator
modules/commander
+1
View File
@@ -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
+1
View File
@@ -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
@@ -38,6 +38,7 @@ set(config_module_list
modules/sensors
modules/dataman
modules/sdlog2
modules/logger
modules/simulator
modules/commander
modules/navigator
+1
View File
@@ -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
+1 -1
View File
@@ -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
+1
View File
@@ -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
+1
View File
@@ -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
+1
View File
@@ -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
+18 -19
View File
@@ -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
+52
View File
@@ -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 :
+127
View File
@@ -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;
};
}
+237
View File
@@ -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, &param);
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;
}
}
}
}
+74
View File
@@ -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;
};
}
}
File diff suppressed because it is too large Load Diff
+96
View File
@@ -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;
}
}
+12
View File
@@ -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);