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