diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 1037e7fec5..403dab2134 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -36,13 +36,20 @@ #include #include #include +#include #include #include +#include +#include +#include + #include #include #include +#define GPS_EPOCH_SECS ((time_t)1234567890ULL) + //#define DBGPRINT //write status output every few seconds #if defined(DBGPRINT) @@ -187,6 +194,7 @@ void Logger::run_trampoline(int argc, char *argv[]) int log_buffer_size = 12 * 1024; bool log_on_start = false; bool error_flag = false; + bool log_name_timestamp = false; int myoptind = 1; int ch; @@ -220,6 +228,10 @@ void Logger::run_trampoline(int argc, char *argv[]) } break; + case 't': + log_name_timestamp = true; + break; + case '?': error_flag = true; break; @@ -236,7 +248,7 @@ void Logger::run_trampoline(int argc, char *argv[]) return; } - logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start); + logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start, log_name_timestamp); #if defined(DBGPRINT) && defined(__PX4_NUTTX) struct mallinfo alloc_info = mallinfo(); @@ -300,11 +312,14 @@ struct message_parameter_header_s { static constexpr size_t MAX_DATA_SIZE = 740; -Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start) : +Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start, + bool log_name_timestamp) : _log_on_start(log_on_start), + _log_name_timestamp(log_name_timestamp), _writer(buffer_size), _log_interval(log_interval) { + _log_utc_offset = param_find("SDLOG_UTC_OFFSET"); } Logger::~Logger() @@ -634,40 +649,52 @@ void Logger::run() } } -int Logger::create_log_dir() +int Logger::create_log_dir(tm *tt) { /* 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); + if (tt) { + int n = snprintf(_log_dir, sizeof(_log_dir), "%s/", LOG_ROOT); + strftime(_log_dir + n, sizeof(_log_dir) - n, "%Y-%m-%d", tt); 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); + if (mkdir_ret != OK && errno != EEXIST) { + PX4_ERR("failed creating new dir: %s", _log_dir); return -1; } - /* dir exists already */ - dir_number++; - continue; - } + } else { + uint16_t dir_number = 1; // start with dir sess001 - 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; + /* look for the next dir that does not exist */ + while (!_has_log_dir && 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_DEBUG("log dir created: %s", _log_dir); + _has_log_dir = true; + + } else if (errno != EEXIST) { + PX4_ERR("failed creating new dir: %s (%i)", _log_dir, errno); + return -1; + } + + /* dir exists already */ + dir_number++; + } + + 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_ERR("All %d possible dirs exist already", MAX_NO_LOGFOLDER); + return -1; + } + + _has_log_dir = true; } - /* print logging path, important to find log file later */ - //mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } @@ -679,37 +706,96 @@ bool Logger::file_exist(const char *filename) int Logger::get_log_file_name(char *file_name, size_t file_name_size) { - uint16_t file_number = 1; // start with file log001 + tm tt; + bool time_ok = false; - /* 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 (_log_name_timestamp) { + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.ulg */ + time_ok = get_log_time(&tt, false); + } - if (!file_exist(file_name)) { - break; + if (time_ok) { + if (create_log_dir(&tt)) { + return -1; } - file_number++; - } + char log_file_name[64] = ""; + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.ulg", &tt); + snprintf(file_name, file_name_size, "%s/%s", _log_dir, log_file_name); - 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; + } else { + if (create_log_dir(nullptr)) { + return -1; + } + + 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 */ + return -1; + } } + return 0; } -void Logger::start_log() +bool Logger::get_log_time(struct tm *tt, bool boot_time) { - PX4_INFO("start log"); + int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - if (create_log_dir()) { - return; + if (vehicle_gps_position_sub < 0) { + return false; } + /* Get the latest GPS publication */ + vehicle_gps_position_s gps_pos; + + if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) < 0) { + orb_unsubscribe(vehicle_gps_position_sub); + return false; + } + + orb_unsubscribe(vehicle_gps_position_sub); + time_t utc_time_sec = gps_pos.time_utc_usec / 1e6; + + if (gps_pos.fix_type < 2 || utc_time_sec < GPS_EPOCH_SECS) { + return false; + } + + /* strip the time elapsed since boot */ + if (boot_time) { + utc_time_sec -= hrt_absolute_time() / 1e6; + } + + int32_t utc_offset = 0; + + if (_log_utc_offset != PARAM_INVALID) { + param_get(_log_utc_offset, &utc_offset); + } + + /* apply utc offset */ + utc_time_sec += utc_offset * 60; + + return gmtime_r(&utc_time_sec, tt) != nullptr; +} + +void Logger::start_log() +{ + PX4_INFO("start log"); + char file_name[64] = ""; if (get_log_file_name(file_name, sizeof(file_name))) { diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 99ddaf89c5..7ce35bf816 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -38,9 +38,9 @@ #include #include #include -#include #include #include +#include extern "C" __EXPORT int logger_main(int argc, char *argv[]); @@ -74,7 +74,7 @@ struct LoggerSubscription { class Logger { public: - Logger(size_t buffer_size, unsigned log_interval, bool log_on_start); + Logger(size_t buffer_size, unsigned log_interval, bool log_on_start, bool log_name_timestamp); ~Logger(); @@ -93,10 +93,18 @@ private: void run(); - int create_log_dir(); + /** + * Create logging directory + * @param tt if not null, use it for the directory name + * @return 0 on success + */ + int create_log_dir(tm *tt); static bool file_exist(const char *filename); + /** + * Get log file name with directory (create it if necessary) + */ int get_log_file_name(char *file_name, size_t file_name_size); void start_log(); @@ -121,6 +129,14 @@ private: */ bool write_wait(void *ptr, size_t size); + /** + * Get the time for log file name + * @param tt returned time + * @param boot_time use time when booted instead of current time + * @return true on success, false otherwise (eg. if no gps) + */ + bool get_log_time(struct tm *tt, bool boot_time = false); + static constexpr size_t MAX_TOPICS_NUM = 128; /**< Maximum number of logged topics */ static constexpr unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ @@ -132,6 +148,7 @@ private: bool _task_should_exit = true; char _log_dir[64]; + bool _has_log_dir = false; bool _enabled = false; // statistics @@ -141,10 +158,12 @@ private: size_t _write_dropouts = 0; ///< failed buffer writes due to buffer overflow size_t _high_water = 0; ///< maximum used write buffer - bool _log_on_start; + const bool _log_on_start; + const bool _log_name_timestamp; Array _subscriptions; LogWriter _writer; uint32_t _log_interval; + param_t _log_utc_offset; }; Logger *logger_ptr = nullptr;