From 3be252289aaee8652ca06235972b793d94be0741 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 16 Oct 2017 14:52:02 +0200 Subject: [PATCH] logger: use orb_exists to check if we need to subscribe to the first instance To keep track of the configured interval, we store it as negative file descriptor, until we do the subscription. This frees up a considerable amount of file descriptors in most use-cases. --- src/modules/logger/logger.cpp | 116 ++++++++++++++++++++++------------ src/modules/logger/logger.h | 20 ++++-- 2 files changed, 92 insertions(+), 44 deletions(-) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 4faf307fc2..ac108217f9 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -425,38 +425,48 @@ bool Logger::request_stop_static() return true; } -int Logger::add_topic(const orb_metadata *topic) +LoggerSubscription* Logger::add_topic(const orb_metadata *topic) { - int fd = -1; + LoggerSubscription *subscription = nullptr; size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':' if (fields_len > sizeof(ulog_message_format_s::format)) { PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len, sizeof(ulog_message_format_s::format)); - return -1; + return nullptr; } - fd = orb_subscribe(topic); + int fd = -1; + // Only subscribe to the topic now if it's published. If published later on, we'll dynamically + // add the subscription then + if (orb_exists(topic, 0) == 0) { + fd = orb_subscribe(topic); - if (fd < 0) { - PX4_WARN("logger: %s subscribe failed (%i)", topic->o_name, errno); - return -1; + if (fd < 0) { + PX4_WARN("logger: %s subscribe failed (%i)", topic->o_name, errno); + return nullptr; + } + } else { + PX4_DEBUG("Topic %s does not exist. Not subscribing (yet)", topic->o_name); } - if (!_subscriptions.push_back(LoggerSubscription(fd, topic))) { + if (_subscriptions.push_back(LoggerSubscription(fd, topic))) { + subscription = &_subscriptions[_subscriptions.size() - 1]; + } else { PX4_WARN("logger: failed to add topic. Too many subscriptions"); - orb_unsubscribe(fd); - fd = -1; + if (fd >= 0) { + orb_unsubscribe(fd); + } } - return fd; + return subscription; } -int Logger::add_topic(const char *name, unsigned interval = 0) +bool Logger::add_topic(const char *name, unsigned interval) { const orb_metadata **topics = orb_get_topics(); - int fd = -1; + LoggerSubscription *subscription = nullptr; for (size_t i = 0; i < orb_topics_count(); i++) { if (strcmp(name, topics[i]->o_name) == 0) { @@ -467,26 +477,35 @@ int Logger::add_topic(const char *name, unsigned interval = 0) if (_subscriptions[j].metadata == topics[i]) { PX4_DEBUG("logging topic %s, interval: %i, already added, only setting interval", topics[i]->o_name, interval); - fd = _subscriptions[j].fd[0]; + subscription = &_subscriptions[j]; already_added = true; break; } } if (!already_added) { - fd = add_topic(topics[i]); + subscription = add_topic(topics[i]); PX4_DEBUG("logging topic: %s, interval: %i", topics[i]->o_name, interval); break; } } } - // 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 && fd >= 0) { - orb_set_interval(fd, interval); + // if we poll on a topic, we don't use the interval and let the polled topic define the maximum interval + if (_polling_topic_meta) { + interval = 0; } - return fd; + if (subscription) { + if (subscription->fd[0] >= 0) { + orb_set_interval(subscription->fd[0], interval); + } else { + // store the interval: use a value < 0 to know it's not a valid fd + subscription->fd[0] = -interval - 1; + } + } + + return subscription; } bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer, bool try_to_subscribe) @@ -496,27 +515,13 @@ bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, if (handle < 0 && try_to_subscribe) { - if (OK == orb_exists(sub.metadata, multi_instance)) { - handle = orb_subscribe_multi(sub.metadata, multi_instance); + if (try_to_subscribe_topic(sub, multi_instance)) { - //PX4_INFO("subscribed to instance %d of topic %s", multi_instance, sub.metadata->o_name); + write_add_logged_msg(sub, multi_instance); /* copy first data */ - if (handle >= 0) { - write_add_logged_msg(sub, multi_instance); - - /* set to the same interval as the first instance */ - unsigned int interval; - - if (orb_get_interval(sub.fd[0], &interval) == 0 && interval > 0) { - orb_set_interval(handle, interval); - } - - /* It can happen that orb_exists returns true, even if there is no publisher (but another subscriber). - * We catch this here, because orb_copy will fail in this case. */ - if (orb_copy(sub.metadata, handle, buffer) == PX4_OK) { - updated = true; - } + if (orb_copy(sub.metadata, handle, buffer) == PX4_OK) { + updated = true; } } @@ -531,6 +536,39 @@ bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, return updated; } +bool Logger::try_to_subscribe_topic(LoggerSubscription &sub, int multi_instance) +{ + bool ret = false; + if (OK == orb_exists(sub.metadata, multi_instance)) { + + unsigned int interval; + + if (multi_instance == 0) { + // the first instance and no subscription yet: this means we stored the negative interval as fd + interval = (unsigned int) (-(sub.fd[0] + 1)); + } else { + // set to the same interval as the first instance + if (orb_get_interval(sub.fd[0], &interval) != 0) { + interval = 0; + } + } + + int &handle = sub.fd[multi_instance]; + handle = orb_subscribe_multi(sub.metadata, multi_instance); + + if (handle >= 0) { + PX4_DEBUG("subscribed to instance %d of topic %s", multi_instance, sub.metadata->o_name); + if (interval > 0) { + orb_set_interval(handle, interval); + } + ret = true; + } else { + PX4_ERR("orb_subscribe_multi %s failed (%i)", sub.metadata->o_name, errno); + } + } + return ret; +} + void Logger::add_default_topics() { #ifdef CONFIG_ARCH_BOARD_SITL @@ -674,7 +712,7 @@ int Logger::add_topics_from_file(const char *fname) } /* add topic with specified interval */ - if (add_topic(topic_name, interval) >= 0) { + if (add_topic(topic_name, interval)) { ntopics++; } else { @@ -1112,7 +1150,7 @@ void Logger::run() //unsubscribe for (LoggerSubscription &sub : _subscriptions) { for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { - if (sub.fd[instance] != -1) { + if (sub.fd[instance] >= 0) { orb_unsubscribe(sub.fd[instance]); sub.fd[instance] = -1; } diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 3b7d0c0b12..50d5915763 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -76,7 +76,8 @@ inline bool operator&(SDLogProfileMask a, SDLogProfileMask b) } struct LoggerSubscription { - int fd[ORB_MULTI_MAX_INSTANCES]; + int fd[ORB_MULTI_MAX_INSTANCES]; ///< uorb subscription. The first fd is also used to store the interval if + /// not subscribed yet (-interval - 1) uint16_t msg_ids[ORB_MULTI_MAX_INSTANCES]; const orb_metadata *metadata = nullptr; @@ -135,14 +136,17 @@ public: * (because it does not write an ADD_LOGGED_MSG message). * @param name topic name * @param interval limit rate if >0, otherwise log as fast as the topic is updated. - * @return -1 on error, file descriptor otherwise + * @return true on success */ - int add_topic(const char *name, unsigned interval); + bool add_topic(const char *name, unsigned interval = 0); /** - * add a logged topic (called by add_topic() above) + * add a logged topic (called by add_topic() above). + * In addition, it subscribes to the first instance of the topic, if it's advertised, + * and sets the file descriptor of LoggerSubscription accordingly + * @return the newly added subscription on success, nullptr otherwise */ - int add_topic(const orb_metadata *topic); + LoggerSubscription *add_topic(const orb_metadata *topic); /** * request the logger thread to stop (this method does not block). @@ -251,6 +255,12 @@ private: inline bool copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer, bool try_to_subscribe); + /** + * Check if a topic instance exists and subscribe to it + * @return true when topic exists and subscription successful + */ + bool try_to_subscribe_topic(LoggerSubscription &sub, int multi_instance); + /** * Write exactly one ulog message to the logger and handle dropouts. * Must be called with _writer.lock() held.