diff --git a/Tools/msg/templates/uorb/msg.cpp.em b/Tools/msg/templates/uorb/msg.cpp.em index b77c013b91..4ffe4c38d1 100644 --- a/Tools/msg/templates/uorb/msg.cpp.em +++ b/Tools/msg/templates/uorb/msg.cpp.em @@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) #include #include +@{ +queue_length = 1 +for constant in spec.constants: + if constant.name == 'ORB_QUEUE_LENGTH': + queue_length = constant.val +}@ + @[for topic in topics]@ static_assert(static_cast(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch"); -ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast(ORB_ID::@topic)); +ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast(ORB_ID::@topic), @queue_length); @[end for] void print_message(const orb_metadata *meta, const @uorb_struct& message) diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index fc06a2da3c..1630054149 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -53,6 +53,10 @@ struct orb_metadata { const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ uint32_t message_hash; /**< Hash over all fields for message compatibility checks */ orb_id_size_t o_id; /**< ORB_ID enum */ +#ifdef CONFIG_ORB_COMMUNICATOR + uint8_t o_queue; /**< queue size */ +#endif + }; typedef const struct orb_metadata *orb_id_t; @@ -102,8 +106,20 @@ typedef const struct orb_metadata *orb_id_t; * @param _size_no_padding Struct size w/o padding at the end * @param _message_hash 32 bit message hash over all fields * @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status + * @param _queue_size Queue size for remote topics in communicator interface */ -#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum) \ +#ifdef CONFIG_ORB_COMMUNICATOR +#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum, _queue_size) \ + const struct orb_metadata __orb_##_name = { \ + #_name, \ + sizeof(_struct), \ + _size_no_padding, \ + _fields, \ + _orb_id_enum, \ + _queue_size \ + }; struct hack +#else +#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum, _queue_size) \ const struct orb_metadata __orb_##_name = { \ #_name, \ sizeof(_struct), \ @@ -111,6 +127,7 @@ typedef const struct orb_metadata *orb_id_t; _message_hash, \ _orb_id_enum \ }; struct hack +#endif __BEGIN_DECLS diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 5b5530694b..c9ff7670ae 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -602,6 +602,22 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) { PX4_DEBUG("entering process_remote_topic: name: %s", topic_name); + // First make sure this is a valid topic + const struct orb_metadata *const *topic_list = orb_get_topics(); + orb_id_t topic_ptr = nullptr; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(topic_list[i]->o_name, topic_name) == 0) { + topic_ptr = topic_list[i]; + break; + } + } + + if (! topic_ptr) { + PX4_ERR("process_remote_topic meta not found for %s\n", topic_name); + return -1; + } + // Look to see if we already have a node for this topic char nodepath[orb_maxpath]; int ret = uORB::Utils::node_mkpath(nodepath, topic_name); @@ -613,7 +629,8 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); if (node) { - PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name); + PX4_DEBUG("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name); + node->update_queue_size(topic_ptr->o_queue); node->mark_as_advertised(); _remote_topics.insert(topic_name); return 0; @@ -622,27 +639,9 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) } // We didn't find a node so we need to create it via an advertisement - const struct orb_metadata *const *topic_list = orb_get_topics(); - orb_id_t topic_ptr = nullptr; - - for (size_t i = 0; i < orb_topics_count(); i++) { - if (strcmp(topic_list[i]->o_name, topic_name) == 0) { - topic_ptr = topic_list[i]; - break; - } - } - - if (topic_ptr) { - PX4_INFO("Advertising remote topic %s", topic_name); - _remote_topics.insert(topic_name); - // Add some queue depth when advertising remote topics. These - // topics may get aggregated and thus delivered in a batch that - // requires some buffering in a queue. - orb_advertise(topic_ptr, nullptr, 5); - - } else { - PX4_INFO("process_remote_topic meta not found for %s\n", topic_name); - } + PX4_DEBUG("Advertising remote topic %s", topic_name); + _remote_topics.insert(topic_name); + orb_advertise(topic_ptr, nullptr, topic_ptr->o_queue); return 0; } @@ -663,8 +662,11 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName) PX4_DEBUG("DeviceNode(%s) not created yet", messageName); } else { - // node is present. - node->process_add_subscription(); + // node is present. But don't send any data to it if it + // is a node advertised by the remote side + if (_remote_topics.find(messageName) == false) { + node->process_add_subscription(); + } } } else {