From 0f30db08c0d44e753005b2a40fef8900ed5dba33 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jan 2014 15:44:03 +0100 Subject: Small documentation correction --- src/systemcmds/tests/tests.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 223edc14a..82de05dff 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 -- cgit v1.2.3 From 4d7e99fd6c47cb94d63a118d5557eefbe6df8f2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 01:09:58 +0100 Subject: Writing RSSI field not only in servo rail topic --- src/drivers/px4io/px4io.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ca35d2f2..7cc7d3b6d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -270,7 +270,8 @@ private: orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety - actuator_outputs_s _outputs; /// 0) { - orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); } else { - _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + _to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status); } } @@ -1450,6 +1451,11 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } + /* set RSSI */ + + // XXX the correct scaling needs to be validated here + rc_val.rssi = _servorail_status.rssi_v / 3.3f; + /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); -- cgit v1.2.3 From dd9df7b1b0974a9838d3e21842a0d90f3eff54d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 01:11:36 +0100 Subject: RSSI field init --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7cc7d3b6d..dede5976d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1454,7 +1454,7 @@ PX4IO::io_publish_raw_rc() /* set RSSI */ // XXX the correct scaling needs to be validated here - rc_val.rssi = _servorail_status.rssi_v / 3.3f; + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; /* lazily advertise on first publication */ if (_to_input_rc == 0) { -- cgit v1.2.3 From 40a0ac5736c35eeecb7e73050e5c685bf3195361 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 20:59:15 +0100 Subject: sdlog2: use GPS time for naming log dirs and files, some fixes --- src/modules/sdlog2/sdlog2.c | 238 +++++++++++++++++++++++++++----------------- 1 file changed, 148 insertions(+), 90 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 06b1eddaa..e9d809834 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -108,7 +108,7 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; @@ -122,14 +122,17 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char folder_path[64]; +static char log_dir[32]; /* statistics counters */ -static unsigned long log_bytes_written = 0; static uint64_t start_time = 0; +static unsigned long log_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; +/* GPS time, used for log files naming */ +static uint64_t gps_time = 0; + /* current state of logging */ static bool logging_enabled = false; /* enable logging on start (-e option) */ @@ -138,11 +141,14 @@ static bool log_on_start = false; static bool log_when_armed = false; /* delay = 1 / rate (rate defined by -r option) */ static useconds_t sleep_delay = 0; +/* use date/time for naming directories and files (-t option) */ +static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; static pthread_t logwriter_pthread = 0; +static pthread_attr_t logwriter_attr; /** * Log buffer writing thread. Open and close file here. @@ -203,9 +209,9 @@ static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); /** - * Create folder for current logging session. Store folder name in 'log_folder'. + * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logfolder(void); +static int create_logdir(void); /** * Select first free log file name and open it. @@ -218,11 +224,12 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\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-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -280,82 +287,101 @@ int sdlog2_main(int argc, char *argv[]) exit(1); } -int create_logfolder() +int create_log_dir() { - /* make folder on sdcard */ - uint16_t folder_number = 1; // start with folder sess001 + /* create dir on sdcard if needed */ + uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - /* look for the next folder that does not exist */ - while (folder_number <= MAX_NO_LOGFOLDER) { - /* set up folder path: e.g. /fs/microsd/sess001 */ - sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ - - if (mkdir_ret == 0) { - /* folder does not exist, success */ + if (log_name_timestamp && gps_time != 0) { + /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", mountpoint); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { + warn("failed creating new dir"); + return -1; + } + } else { + /* 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", mountpoint, dir_number); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret != 0) { + if (errno == EEXIST) { + /* dir exists already */ + dir_number++; + continue; + } else { + warn("failed creating new dir"); + return -1; + } + } + /* dir does not exist, success */ break; + } - } else if (mkdir_ret == -1) { - /* folder exists already */ - folder_number++; - continue; - - } else { - warn("failed creating new folder"); + 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 */ + warnx("all %d possible dirs exist already.", MAX_NO_LOGFOLDER); return -1; } } - if (folder_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 */ - warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER); - return -1; - } - + /* print logging path, important to find log file later */ + warnx("logging to dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] logging to dir: %s", log_dir); return 0; } int open_logfile() { - /* make folder on sdcard */ - uint16_t file_number = 1; // start with file log001 - /* string to hold the path to the log */ - char path_buf[64] = ""; - - int fd = 0; - - /* look for the next file that does not exist */ - while (file_number <= MAX_NO_LOGFILE) { - /* set up file path: e.g. /fs/microsd/sess001/log001.bin */ - sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number); - - if (file_exist(path_buf)) { + char log_file[64] = ""; + + if (log_name_timestamp && gps_time != 0) { + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + int n = snprintf(log_file, sizeof(log_file), "%s/", log_dir); + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + n += strftime(log_file + n, sizeof(log_file) - n, "%H_%M_%S.bin", &t); + } else { + 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.bin */ + snprintf(log_file, sizeof(log_file), "%s/log%03u.bin", log_dir, file_number); + + if (!file_exist(log_file)) { + break; + } file_number++; - continue; } - fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); - - if (fd == 0) { - warn("opening %s failed", path_buf); + 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 */ + warnx("all %d possible files exist already.", MAX_NO_LOGFILE); + return -1; } - - warnx("logging to: %s.", path_buf); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); - - return fd; } - 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 */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); - return -1; + int fd = open(log_file, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd < 0) { + warn("failed opening %s", log_file); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening %s", log_file); + } else { + warnx("logging to: %s", log_file); + mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", log_file); } - return 0; + return fd; } static void *logwriter_thread(void *arg) @@ -363,10 +389,13 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - int log_fd = open_logfile(); + if (log_fd < 0) + return; + + struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; + /* write log messages formats, version and parameters */ log_bytes_written += write_formats(log_fd); log_bytes_written += write_version(log_fd); @@ -443,7 +472,7 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - return OK; + return; } void sdlog2_start_log() @@ -451,6 +480,22 @@ void sdlog2_start_log() warnx("start logging."); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + /* create log dir if needed */ + if (create_log_dir() != 0) { + mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + errx(1, "error creating log dir"); + } + + const char *converter_in = "/etc/logging/conv.zip"; + char *converter_out = malloc(120); + sprintf(converter_out, "%s/conv.zip", log_dir); + + if (file_copy(converter_in, converter_out)) { + warnx("unable to copy conversion scripts"); + } + + free(converter_out); + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -458,25 +503,23 @@ void sdlog2_start_log() log_msgs_skipped = 0; /* initialize log buffer emptying thread */ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); + pthread_attr_init(&logwriter_attr); struct sched_param param; /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + (void)pthread_attr_setschedparam(&logwriter_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&logwriter_attr, 2048); logwriter_should_exit = false; /* start log buffer emptying thread */ - if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { + if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } logging_enabled = true; - // XXX we have to destroy the attr at some point } void sdlog2_stop_log() @@ -501,6 +544,7 @@ void sdlog2_stop_log() } logwriter_pthread = 0; + pthread_attr_destroy(&logwriter_attr); sdlog2_status(); } @@ -594,12 +638,19 @@ int sdlog2_thread_main(int argc, char *argv[]) /* log buffer size */ int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; + logging_enabled = false; + log_on_start = false; + log_when_armed = false; + log_name_timestamp = false; + + flag_system_armed = false; + /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); @@ -632,6 +683,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_when_armed = true; break; + case 't': + log_name_timestamp = true; + break; + case '?': if (optopt == 'c') { warnx("Option -%c requires an argument.", optopt); @@ -649,27 +704,12 @@ int sdlog2_thread_main(int argc, char *argv[]) } } + gps_time = 0; + if (!file_exist(mountpoint)) { errx(1, "logging mount point %s not present, exiting.", mountpoint); } - if (create_logfolder()) { - errx(1, "unable to create logging folder, exiting."); - } - - const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", folder_path); - - if (file_copy(converter_in, converter_out)) { - errx(1, "unable to copy conversion scripts, exiting."); - } - - free(converter_out); - - /* only print logging path, important to find log file later */ - warnx("logging to directory: %s", folder_path); - /* initialize log buffer with specified size */ warnx("log buffer size: %i bytes.", log_buffer_size); @@ -919,15 +959,22 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t differential_pressure_counter = 0; /* enable logging on start if needed */ - if (log_on_start) + if (log_on_start) { + /* check GPS topic to get GPS time */ + if (log_name_timestamp) { + if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) { + gps_time = buf.gps_pos.time_gps_usec; + } + } sdlog2_start_log(); + } while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; /* poll all topics if logging enabled or only management (first 2) if not */ - int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); + int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ if (poll_ret < 0) { @@ -960,6 +1007,17 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } + /* --- GPS POSITION - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + + if (log_name_timestamp) { + gps_time = buf.gps_pos.time_gps_usec; + } + + handled_topics++; + } + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } @@ -988,7 +1046,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; -- cgit v1.2.3 From 5e3c365cd4809def0c5b21136a1b5741a98ae35e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 22:56:24 +0100 Subject: rc: use sdlog2 -t option sdlog2: move all logs and conv.zip to "log" dir, messages cleanup --- ROMFS/px4fmu_common/init.d/rc.logging | 4 +- src/modules/sdlog2/sdlog2.c | 129 +++++++++++++++++----------------- 2 files changed, 68 insertions(+), 65 deletions(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055..6c02bf227 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -7,8 +7,8 @@ if [ -d /fs/microsd ] then if [ $BOARD == fmuv1 ] then - sdlog2 start -r 50 -a -b 16 + sdlog2 start -r 50 -a -b 16 -t else - sdlog2 start -r 200 -a -b 16 + sdlog2 start -r 200 -a -b 16 -t fi fi diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e9d809834..656575573 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -114,7 +114,7 @@ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; -static const char *mountpoint = "/fs/microsd"; +static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -211,12 +211,12 @@ static void handle_status(struct vehicle_status_s *cmd); /** * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logdir(void); +static int create_log_dir(void); /** * Select first free log file name and open it. */ -static int open_logfile(void); +static int open_log_file(void); static void sdlog2_usage(const char *reason) @@ -298,67 +298,69 @@ int create_log_dir() time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); - int n = snprintf(log_dir, sizeof(log_dir), "%s/", mountpoint); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret != 0 && errno != EEXIST) { - warn("failed creating new dir"); + if (mkdir_ret == OK) { + warnx("log dir created: %s", log_dir); + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); return -1; } } else { /* 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", mountpoint, dir_number); + 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) { - if (errno == EEXIST) { - /* dir exists already */ - dir_number++; - continue; - } else { - warn("failed creating new dir"); - return -1; - } + if (mkdir_ret == 0) { + warnx("log dir created: %s", log_dir); + break; + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; } - /* dir does not exist, success */ - break; + /* 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 */ - warnx("all %d possible dirs exist already.", MAX_NO_LOGFOLDER); + warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER); return -1; } } /* print logging path, important to find log file later */ - warnx("logging to dir: %s", log_dir); - mavlink_log_info(mavlink_fd, "[sdlog2] logging to dir: %s", log_dir); + warnx("log dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } -int open_logfile() +int open_log_file() { /* string to hold the path to the log */ - char log_file[64] = ""; + char log_file_name[16] = ""; + char log_file_path[48] = ""; if (log_name_timestamp && gps_time != 0) { /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - int n = snprintf(log_file, sizeof(log_file), "%s/", log_dir); time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); - n += strftime(log_file + n, sizeof(log_file) - n, "%H_%M_%S.bin", &t); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { 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.bin */ - snprintf(log_file, sizeof(log_file), "%s/log%03u.bin", log_dir, file_number); + snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); - if (!file_exist(log_file)) { + if (!file_exist(log_file_path)) { break; } file_number++; @@ -366,19 +368,19 @@ int open_logfile() 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 */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); + warnx("all %d possible files exist already", MAX_NO_LOGFILE); return -1; } } - int fd = open(log_file, O_CREAT | O_WRONLY | O_DSYNC); + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening %s", log_file); - mavlink_log_info(mavlink_fd, "[sdlog2] failed opening %s", log_file); + warn("failed opening log: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("logging to: %s", log_file); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", log_file); + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -389,7 +391,7 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - int log_fd = open_logfile(); + int log_fd = open_log_file(); if (log_fd < 0) return; @@ -477,7 +479,7 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { - warnx("start logging."); + warnx("start logging"); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); /* create log dir if needed */ @@ -486,16 +488,6 @@ void sdlog2_start_log() errx(1, "error creating log dir"); } - const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", log_dir); - - if (file_copy(converter_in, converter_out)) { - warnx("unable to copy conversion scripts"); - } - - free(converter_out); - /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -524,7 +516,7 @@ void sdlog2_start_log() void sdlog2_stop_log() { - warnx("stop logging."); + warnx("stop logging"); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -632,7 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first."); + warnx("failed to open MAVLink log stream, start mavlink app first"); } /* log buffer size */ @@ -689,32 +681,43 @@ int sdlog2_thread_main(int argc, char *argv[]) case '?': if (optopt == 'c') { - warnx("Option -%c requires an argument.", optopt); + warnx("option -%c requires an argument", optopt); } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.", optopt); + warnx("unknown option `-%c'", optopt); } else { - warnx("Unknown option character `\\x%x'.", optopt); + warnx("unknown option character `\\x%x'", optopt); } default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting."); + errx(1, "exiting"); } } gps_time = 0; - if (!file_exist(mountpoint)) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); + /* create log root dir */ + int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { + err("failed creating log root dir: %s", log_root); } + /* copy conversion scripts */ + const char *converter_in = "/etc/logging/conv.zip"; + char *converter_out = malloc(64); + snprintf(converter_out, 64, "%s/conv.zip", log_root); + if (file_copy(converter_in, converter_out) != OK) { + warn("unable to copy conversion scripts"); + } + free(converter_out); + /* initialize log buffer with specified size */ - warnx("log buffer size: %i bytes.", log_buffer_size); + warnx("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { - errx(1, "can't allocate log buffer, exiting."); + errx(1, "can't allocate log buffer, exiting"); } struct vehicle_status_s buf_status; @@ -935,7 +938,7 @@ int sdlog2_thread_main(int argc, char *argv[]) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__); fdsc_count = fdsc; } @@ -978,7 +981,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* handle the poll result */ if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging."); + warnx("ERROR: poll error, stop logging"); main_thread_should_exit = true; } else if (poll_ret > 0) { @@ -1337,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting."); + warnx("exiting"); thread_running = false; @@ -1350,8 +1353,8 @@ void sdlog2_status() float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("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_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped); + warnx("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_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } /** @@ -1370,7 +1373,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy."); + warnx("failed opening input file to copy"); return 1; } @@ -1378,7 +1381,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy."); + warnx("failed to open output file to copy"); return 1; } @@ -1389,7 +1392,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file."); + warnx("error writing file"); ret = 1; break; } -- cgit v1.2.3 From 47c226988ccf0e90bda9fe7c106bcdaf8b2e67fd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 22:56:57 +0100 Subject: sdlog2: code style fixed --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 656575573..2319dbb3b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -301,12 +301,15 @@ int create_log_dir() int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret == OK) { warnx("log dir created: %s", log_dir); + } else if (errno != EEXIST) { warn("failed creating new dir: %s", log_dir); return -1; } + } else { /* look for the next dir that does not exist */ while (dir_number <= MAX_NO_LOGFOLDER) { @@ -317,10 +320,12 @@ int create_log_dir() if (mkdir_ret == 0) { warnx("log dir created: %s", log_dir); break; + } else if (errno != EEXIST) { warn("failed creating new dir: %s", log_dir); return -1; } + /* dir exists already */ dir_number++; continue; @@ -352,8 +357,10 @@ int open_log_file() gmtime_r(&gps_time_sec, &t); strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); + } else { 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.bin */ @@ -363,6 +370,7 @@ int open_log_file() if (!file_exist(log_file_path)) { break; } + file_number++; } @@ -378,6 +386,7 @@ int open_log_file() if (fd < 0) { warn("failed opening log: %s", log_file_name); mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + } else { warnx("log file: %s", log_file_name); mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); @@ -605,8 +614,8 @@ int write_parameters(int fd) } case PARAM_TYPE_FLOAT: - param_get(param, &value); - break; + param_get(param, &value); + break; default: break; @@ -700,6 +709,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { err("failed creating log root dir: %s", log_root); } @@ -708,9 +718,11 @@ int sdlog2_thread_main(int argc, char *argv[]) const char *converter_in = "/etc/logging/conv.zip"; char *converter_out = malloc(64); snprintf(converter_out, 64, "%s/conv.zip", log_root); + if (file_copy(converter_in, converter_out) != OK) { warn("unable to copy conversion scripts"); } + free(converter_out); /* initialize log buffer with specified size */ @@ -969,6 +981,7 @@ int sdlog2_thread_main(int argc, char *argv[]) gps_time = buf.gps_pos.time_gps_usec; } } + sdlog2_start_log(); } -- cgit v1.2.3