diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-23 19:41:38 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-23 19:41:38 +0200 |
commit | ffe095646c5984236731c88fabfe2b2b0cedf983 (patch) | |
tree | 19a9e52c74f6b0e6d30025fa9b9b17b6d4c0669a | |
parent | 7d7aaca18c9227e03875dc65a4f0012be5c1e7cf (diff) | |
download | px4-firmware-ffe095646c5984236731c88fabfe2b2b0cedf983.tar.gz px4-firmware-ffe095646c5984236731c88fabfe2b2b0cedf983.tar.bz2 px4-firmware-ffe095646c5984236731c88fabfe2b2b0cedf983.zip |
sdlog2: Set logging rate and extended logging based on parameters (overwrites commandline if non-standard)
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 68 |
1 files changed, 68 insertions, 0 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ff68b5ad6..81c972112 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -98,6 +98,36 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" +/** + * Logging rate. + * + * A value of -1 indicates the commandline argument + * should be obeyed. A value of 0 sets the minimum rate, + * any other value is interpreted as rate in Hertz. This + * parameter is only read out before logging starts (which + * commonly is before arming). + * + * @min -1 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_RATE, -1); + +/** + * Enable extended logging mode. + * + * A value of -1 indicates the commandline argument + * should be obeyed. A value of 0 disables extended + * logging mode, a value of 1 enables it. This + * parameter is only read out before logging starts + * (which commonly is before arming). + * + * @min -1 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_EXT, -1); + #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ } else { \ @@ -814,6 +844,44 @@ int sdlog2_thread_main(int argc, char *argv[]) gps_time = 0; + /* interpret logging params */ + + param_t log_rate_ph = param_find("SDLOG_RATE"); + + if (log_rate_ph != PARAM_INVALID) { + int32_t param_log_rate; + param_get(log_rate_ph, ¶m_log_rate); + + if (param_log_rate > 0) { + + /* we can't do more than ~ 500 Hz, even with a massive buffer */ + if (param_log_rate > 500) { + param_log_rate = 500; + } + + sleep_delay = 1000000 / param_log_rate; + } else if (param_log_rate == 0) { + /* we need at minimum 10 Hz to be able to see anything */ + sleep_delay = 1000000 / 10; + } + } + + param_t log_ext_ph = param_find("SDLOG_EXT"); + + if (log_ext_ph != PARAM_INVALID) { + + int32_t param_log_extended; + param_get(log_ext_ph, ¶m_log_extended); + + if (param_log_extended > 0) { + _extended_logging = true; + } else if (param_log_extended == 0) { + _extended_logging = false; + } + /* any other value means to ignore the parameter, so no else case */ + + } + /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); |