From 45ffb190e32b8ae8970940c24554e99f0eb2e275 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 1 Mar 2017 12:10:41 +0100 Subject: [PATCH] logger: add -p option to poll on a topic instead of running at fixed rate this will be needed for fast replay. In addition, this option disables the orb interval. It can be removed again once we have time simulation. --- src/modules/logger/logger.cpp | 99 ++++++++++++++++++++++++++++------- src/modules/logger/logger.h | 5 +- 2 files changed, 83 insertions(+), 21 deletions(-) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 976706c310..80c0e165c8 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -189,8 +190,10 @@ void Logger::usage(const char *reason) PX4_WARN("%s\n", reason); } - PX4_INFO("usage: logger {start|stop|on|off|status} [-r ] [-b ] -e -a -t -x -m -q \n" + PX4_INFO("usage: logger {start|stop|on|off|status} [-r ] [-b ]\n" + " [-p ] -e -a -t -x -m -q \n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-p\tPoll on a topic instead of running with fixed rate (Log rate and topic interval are ignored if this is set)\n" "\t-b\tLog buffer size in KiB, default is 12\n" "\t-e\tEnable logging right after start until disarm (otherwise only when armed)\n" "\t-f\tLog until shutdown (implies -e)\n" @@ -265,12 +268,13 @@ void Logger::run_trampoline(int argc, char *argv[]) unsigned int queue_size = 14; //TODO: we might be able to reduce this if mavlink polled on the topic and/or // topic sizes get reduced LogWriter::Backend backend = LogWriter::BackendAll; + const char *poll_topic = nullptr; int myoptind = 1; int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "r:b:etfm:q:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "r:b:etfm:q:p:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(myoptarg, nullptr, 10); @@ -324,6 +328,10 @@ void Logger::run_trampoline(int argc, char *argv[]) break; + case 'p': + poll_topic = myoptarg; + break; + case 'q': queue_size = strtoul(myoptarg, nullptr, 10); @@ -349,7 +357,7 @@ void Logger::run_trampoline(int argc, char *argv[]) return; } - logger_ptr = new Logger(backend, log_buffer_size, log_interval, log_on_start, + logger_ptr = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_on_start, log_until_shutdown, log_name_timestamp, queue_size); #if defined(DBGPRINT) && defined(__PX4_NUTTX) @@ -376,8 +384,8 @@ void Logger::run_trampoline(int argc, char *argv[]) } -Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, bool log_on_start, - bool log_until_shutdown, bool log_name_timestamp, unsigned int queue_size) : +Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, + bool log_on_start, bool log_until_shutdown, bool log_name_timestamp, unsigned int queue_size) : _arm_override(false), _log_on_start(log_on_start), _log_until_shutdown(log_until_shutdown), @@ -388,6 +396,21 @@ Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_inte { _log_utc_offset = param_find("SDLOG_UTC_OFFSET"); _sdlog_mode_handle = param_find("SDLOG_MODE"); + + if (poll_topic_name) { + const orb_metadata **topics = orb_get_topics(); + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(poll_topic_name, topics[i]->o_name) == 0) { + _polling_topic_meta = topics[i]; + break; + } + } + + if (!_polling_topic_meta) { + PX4_ERR("Failed to find topic %s", poll_topic_name); + } + } } Logger::~Logger() @@ -436,7 +459,7 @@ int Logger::add_topic(const orb_metadata *topic) fd = orb_subscribe(topic); if (fd < 0) { - PX4_WARN("logger: orb_subscribe failed"); + PX4_WARN("logger: orb_subscribe failed (%i)", errno); return -1; } @@ -462,8 +485,11 @@ int Logger::add_topic(const char *name, unsigned interval = 0) } } - if (fd >= 0 && interval != 0) { - orb_set_interval(fd, interval); + // if we poll on a topic, we don't set the interval and let the polled topic define the maximum interval + if (!_polling_topic_meta) { + if (fd >= 0 && interval != 0) { + orb_set_interval(fd, interval); + } } return fd; @@ -719,6 +745,10 @@ void Logger::run() max_msg_size = sizeof(ulog_message_logging_s); } + if (_polling_topic_meta && _polling_topic_meta->o_size > max_msg_size) { + max_msg_size = _polling_topic_meta->o_size; + } + if (max_msg_size > _msg_buffer_len) { if (_msg_buffer) { delete[](_msg_buffer); @@ -757,13 +787,21 @@ void Logger::run() memset(&timer_call, 0, sizeof(hrt_call)); px4_sem_t timer_semaphore; px4_sem_init(&timer_semaphore, 0, 0); - /* timer_semaphore use case is a signal */ - px4_sem_setprotocol(&timer_semaphore, SEM_PRIO_NONE); + int polling_topic_sub = -1; - hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_semaphore); + if (_polling_topic_meta) { + polling_topic_sub = orb_subscribe(_polling_topic_meta); + + if (polling_topic_sub < 0) { + PX4_ERR("Failed to subscribe (%i)", errno); + } + + } else { + hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_semaphore); + } // check for new subscription data hrt_abstime next_subscribe_check = 0; @@ -955,14 +993,33 @@ void Logger::run() } - /* - * We wait on the semaphore, which periodically gets updated by a high-resolution timer. - * The simpler alternative would be: - * usleep(max(300, _log_interval - elapsed_time_since_loop_start)); - * And on linux this is quite accurate as well, but under NuttX it is not accurate, - * because usleep() has only a granularity of CONFIG_MSEC_PER_TICK (=1ms). - */ - while (px4_sem_wait(&timer_semaphore) != 0); + // wait for next loop iteration... + if (polling_topic_sub >= 0) { + px4_pollfd_struct_t fds[1]; + fds[0].fd = polling_topic_sub; + fds[0].events = POLLIN; + int pret = px4_poll(fds, 1, 1000); + + if (pret < 0) { + PX4_ERR("poll failed (%i)", pret); + + } else if (pret != 0) { + if (fds[0].revents & POLLIN) { + // need to to an orb_copy so that poll will not return immediately + orb_copy(_polling_topic_meta, polling_topic_sub, _msg_buffer); + } + } + + } else { + /* + * We wait on the semaphore, which periodically gets updated by a high-resolution timer. + * The simpler alternative would be: + * usleep(max(300, _log_interval - elapsed_time_since_loop_start)); + * And on linux this is quite accurate as well, but under NuttX it is not accurate, + * because usleep() has only a granularity of CONFIG_MSEC_PER_TICK (=1ms). + */ + while (px4_sem_wait(&timer_semaphore) != 0); + } } hrt_cancel(&timer_call); @@ -981,6 +1038,10 @@ void Logger::run() } } + if (polling_topic_sub >= 0) { + orb_unsubscribe(polling_topic_sub); + } + if (_mavlink_log_pub) { orb_unadvertise(_mavlink_log_pub); _mavlink_log_pub = nullptr; diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 066a1dafc6..6da20955df 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -82,8 +82,8 @@ struct LoggerSubscription { class Logger { public: - Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, bool log_on_start, - bool log_until_shutdown, bool log_name_timestamp, unsigned int queue_size); + Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, + bool log_on_start, bool log_until_shutdown, bool log_name_timestamp, unsigned int queue_size); ~Logger(); @@ -252,6 +252,7 @@ private: Array _subscriptions; LogWriter _writer; uint32_t _log_interval; + const orb_metadata *_polling_topic_meta = nullptr; ///< if non-null, poll on this topic instead of sleeping param_t _log_utc_offset; orb_advert_t _mavlink_log_pub = nullptr; uint16_t _next_topic_id = 0; ///< id of next subscribed ulog topic