diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-06-07 22:02:40 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-06-07 22:02:40 +0400 |
commit | 59b26eca48212f13a467724f9445169b78d6c70a (patch) | |
tree | bf5bd1ebad966f1490e29c3ded7aeec5b3536780 /src/modules | |
parent | d39999425d92997ca9db77305d5c87268c601d49 (diff) | |
download | px4-firmware-59b26eca48212f13a467724f9445169b78d6c70a.tar.gz px4-firmware-59b26eca48212f13a467724f9445169b78d6c70a.tar.bz2 px4-firmware-59b26eca48212f13a467724f9445169b78d6c70a.zip |
sdlog2 -b option (log buffer size) added, minor cleanup
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/sdlog2/logbuffer.c | 3 | ||||
-rw-r--r-- | src/modules/sdlog2/logbuffer.h | 4 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 50 |
3 files changed, 40 insertions, 17 deletions
diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 4205bcf20..8aaafaf31 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -45,12 +45,13 @@ #include "logbuffer.h" -void logbuffer_init(struct logbuffer_s *lb, int size) +int logbuffer_init(struct logbuffer_s *lb, int size) { lb->size = size; lb->write_ptr = 0; lb->read_ptr = 0; lb->data = malloc(lb->size); + return (lb->data == 0) ? ERROR : OK; } int logbuffer_count(struct logbuffer_s *lb) diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h index e9635e5e7..31521f722 100644 --- a/src/modules/sdlog2/logbuffer.h +++ b/src/modules/sdlog2/logbuffer.h @@ -46,14 +46,14 @@ #include <stdbool.h> struct logbuffer_s { - // all pointers are in bytes + // pointers and size are in bytes int write_ptr; int read_ptr; int size; char *data; }; -void logbuffer_init(struct logbuffer_s *lb, int size); +int logbuffer_init(struct logbuffer_s *lb, int size); int logbuffer_count(struct logbuffer_s *lb); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6fc430fc9..4ad339aed 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -94,9 +94,9 @@ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ - fds[fdsc_count].fd = subs.##_var##_sub; \ - fds[fdsc_count].events = POLLIN; \ - fdsc_count++; + fds[fdsc_count].fd = subs.##_var##_sub; \ + fds[fdsc_count].events = POLLIN; \ + fdsc_count++; //#define SDLOG2_DEBUG @@ -107,7 +107,7 @@ 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_LOGFILE = 999; /**< Maximum number of log files */ -static const int LOG_BUFFER_SIZE = 8192; +static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; @@ -207,8 +207,9 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-r\tLog buffer size in KBytes, 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"); } @@ -529,18 +530,18 @@ int sdlog2_thread_main(int argc, char *argv[]) warnx("failed to open MAVLink log stream, start mavlink app first."); } - /* log every n'th value (skip three per default) */ - int skip_value = 3; + /* log buffer size */ + int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { switch (ch) { case 'r': { - unsigned r = strtoul(optarg, NULL, 10); + unsigned long r = strtoul(optarg, NULL, 10); if (r == 0) { sleep_delay = 0; @@ -551,6 +552,20 @@ int sdlog2_thread_main(int argc, char *argv[]) } break; + case 'b': { + unsigned long s = strtoul(optarg, NULL, 10); + + if (s < 1) { + s = 1; + + } else if (s > 640) { + s = 640; + } + + log_buffer_size = 1024 * s; + } + break; + case 'e': log_on_start = true; break; @@ -572,7 +587,7 @@ int sdlog2_thread_main(int argc, char *argv[]) default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting"); + errx(1, "exiting."); } } @@ -580,12 +595,20 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "logging mount point %s not present, exiting.", mountpoint); } - if (create_logfolder()) + if (create_logfolder()) { errx(1, "unable to create logging folder, exiting."); + } /* 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); + + if (OK != logbuffer_init(&lb, log_buffer_size)) { + errx(1, "can't allocate log buffer, exiting."); + } + /* file descriptors to wait for */ struct pollfd fds_control[2]; @@ -770,9 +793,6 @@ int sdlog2_thread_main(int argc, char *argv[]) thread_running = true; - /* initialize log buffer with specified size */ - logbuffer_init(&lb, LOG_BUFFER_SIZE); - /* initialize thread synchronization */ pthread_mutex_init(&logbuffer_mutex, NULL); pthread_cond_init(&logbuffer_cond, NULL); @@ -818,9 +838,11 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + if (log_when_armed) { handle_status(&buf_status); } + handled_topics++; } |