diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 486eac9464..e0cc5693ed 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -30,4 +30,5 @@ exec find boards msg src platforms test \ -path src/lib/cdrstream/cyclonedds -prune -o \ -path src/lib/cdrstream/rosidl -prune -o \ -path src/modules/zenoh/zenoh-pico -prune -o \ + -path src/modules/muorb/apps/libfc-sensor-api -prune -o \ -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN 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/msg/OrbTestMedium.msg b/msg/OrbTestMedium.msg index b25ae1c851..43109d49d5 100644 --- a/msg/OrbTestMedium.msg +++ b/msg/OrbTestMedium.msg @@ -4,4 +4,6 @@ int32 val uint8[64] junk +uint8 ORB_QUEUE_LENGTH = 16 + # TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll diff --git a/msg/TransponderReport.msg b/msg/TransponderReport.msg index 6a1ca06418..d5171cf3b3 100644 --- a/msg/TransponderReport.msg +++ b/msg/TransponderReport.msg @@ -47,4 +47,4 @@ uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18 uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19 uint16 ADSB_EMITTER_TYPE_ENUM_END=20 -uint8 ORB_QUEUE_LENGTH = 8 +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index a3d9b02338..73a3671c89 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -74,7 +74,7 @@ void px4_log_initialize(void) log_message.severity = 6; // info strcpy((char *)log_message.text, "initialized uORB logging"); log_message.timestamp = hrt_absolute_time(); - orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, log_message_s::ORB_QUEUE_LENGTH); + orb_log_message_pub = orb_advertise(ORB_ID(log_message), &log_message); } __EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...) diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp index 7547e3b7fa..a0a7cc20cf 100644 --- a/platforms/common/uORB/Publication.hpp +++ b/platforms/common/uORB/Publication.hpp @@ -48,24 +48,6 @@ namespace uORB { -template class DefaultQueueSize -{ -private: - template - static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *) - { - return T::ORB_QUEUE_LENGTH; - } - - template static constexpr uint8_t get_queue_size(...) - { - return 1; - } - -public: - static constexpr unsigned value = get_queue_size(nullptr); -}; - class PublicationBase { public: @@ -97,7 +79,7 @@ protected: /** * uORB publication wrapper class */ -template::value> +template class Publication : public PublicationBase { public: @@ -113,7 +95,7 @@ public: bool advertise() { if (!advertised()) { - _handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE); + _handle = orb_advertise(get_topic(), nullptr); } return advertised(); diff --git a/platforms/common/uORB/PublicationMulti.hpp b/platforms/common/uORB/PublicationMulti.hpp index bc275940b6..6e8863b17d 100644 --- a/platforms/common/uORB/PublicationMulti.hpp +++ b/platforms/common/uORB/PublicationMulti.hpp @@ -51,7 +51,7 @@ namespace uORB /** * Base publication multi wrapper class */ -template::value> +template class PublicationMulti : public PublicationBase { public: @@ -73,7 +73,7 @@ public: { if (!advertised()) { int instance = 0; - _handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE); + _handle = orb_advertise_multi(get_topic(), nullptr, &instance); } return advertised(); diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index c91ac6bcce..a68f3c5dc8 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -118,22 +118,11 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) return uORB::Manager::get_instance()->orb_advertise(meta, data); } -orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size) -{ - return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size); -} - orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) { return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance); } -orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) -{ - return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size); -} - int orb_unadvertise(orb_advert_t handle) { return uORB::Manager::get_instance()->orb_unadvertise(handle); @@ -227,6 +216,14 @@ const char *orb_get_c_type(unsigned char short_type) return nullptr; } +uint8_t orb_get_queue_size(const struct orb_metadata *meta) +{ + if (meta) { + return meta->o_queue; + } + + return 0; +} void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name) { diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index fc06a2da3c..a8964b4d27 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -53,6 +53,8 @@ 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 */ + uint8_t o_queue; /**< queue size */ + }; typedef const struct orb_metadata *orb_id_t; @@ -102,14 +104,16 @@ 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 from topic definition */ -#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum) \ - const struct orb_metadata __orb_##_name = { \ - #_name, \ - sizeof(_struct), \ - _size_no_padding, \ - _message_hash, \ - _orb_id_enum \ +#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum, _queue_size) \ + const struct orb_metadata __orb_##_name = { \ + #_name, \ + sizeof(_struct), \ + _size_no_padding, \ + _message_hash, \ + _orb_id_enum, \ + _queue_size \ }; struct hack __BEGIN_DECLS @@ -135,23 +139,11 @@ typedef void *orb_advert_t; */ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; -/** - * @see uORB::Manager::orb_advertise() - */ -extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, - unsigned int queue_size) __EXPORT; - /** * @see uORB::Manager::orb_advertise_multi() */ extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT; -/** - * @see uORB::Manager::orb_advertise_multi() - */ -extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) __EXPORT; - /** * @see uORB::Manager::orb_unadvertise() */ @@ -160,7 +152,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT; /** * @see uORB::Manager::orb_publish() */ -extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; +extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; /** * Advertise as the publisher of a topic. @@ -241,6 +233,12 @@ extern int orb_get_interval(int handle, unsigned *interval) __EXPORT; */ const char *orb_get_c_type(unsigned char short_type); +/** + * Returns the queue size of a topic + * @param meta orb topic metadata + */ +extern uint8_t orb_get_queue_size(const struct orb_metadata *meta); + /** * Print a topic to console. Do not call this directly, use print_message() instead. * @param meta orb topic metadata diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 112170de6b..e83b3f3e04 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -48,37 +48,10 @@ static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast(filp->f_priv); } -// round up to nearest power of two -// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128 -// Note: When the input value > 128, the output is always 128 -static inline uint8_t round_pow_of_two_8(uint8_t n) -{ - if (n == 0) { - return 1; - } - - // Avoid is already a power of 2 - uint8_t value = n - 1; - - // Fill 1 - value |= value >> 1U; - value |= value >> 2U; - value |= value >> 4U; - - // Unable to round-up, take the value of round-down - if (value == UINT8_MAX) { - value >>= 1U; - } - - return value + 1; -} - -uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, - uint8_t queue_size) : +uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) : CDev(strdup(path)), // success is checked in CDev::init _meta(meta), - _instance(instance), - _queue_size(round_pow_of_two_8(queue_size)) + _instance(instance) { } @@ -186,7 +159,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) /* re-check size */ if (nullptr == _data) { - const size_t data_size = _meta->o_size * _queue_size; + const size_t data_size = _meta->o_size * _meta->o_queue; _data = (uint8_t *) px4_cache_aligned_alloc(data_size); if (_data) { @@ -217,7 +190,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ unsigned generation = _generation.fetch_add(1); - memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size); + memcpy(_data + (_meta->o_size * (generation % _meta->o_queue)), buffer, _meta->o_size); // callbacks for (auto item : _callbacks) { @@ -254,13 +227,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) *(uintptr_t *)arg = (uintptr_t)this; return PX4_OK; - case ORBIOCSETQUEUESIZE: { - lock(); - int ret = update_queue_size(arg); - unlock(); - return ret; - } - case ORBIOCGETINTERVAL: *(unsigned *)arg = filp_to_subscription(filp)->get_interval_us(); return PX4_OK; @@ -389,12 +355,11 @@ uORB::DeviceNode::print_statistics(int max_topic_length) const uint8_t instance = get_instance(); const int8_t sub_count = subscriber_count(); - const uint8_t queue_size = get_queue_size(); unlock(); PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count, - queue_size, get_meta()->o_size, get_devname()); + get_meta()->o_queue, get_meta()->o_size, get_devname()); return true; } @@ -483,21 +448,6 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data } #endif /* CONFIG_ORB_COMMUNICATOR */ -int uORB::DeviceNode::update_queue_size(unsigned int queue_size) -{ - if (_queue_size == queue_size) { - return PX4_OK; - } - - //queue size is limited to 255 for the single reason that we use uint8 to store it - if (_data || _queue_size > queue_size || queue_size > 255) { - return PX4_ERROR; - } - - _queue_size = round_pow_of_two_8(queue_size); - return PX4_OK; -} - unsigned uORB::DeviceNode::get_initial_generation() { ATOMIC_ENTER; diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index c5c5bb22c0..527c5ddf2b 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -62,7 +62,7 @@ class UnitTest; class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode { public: - DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1); + DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path); virtual ~DeviceNode(); // no copy, assignment, move, move assignment @@ -179,15 +179,6 @@ public: void mark_as_advertised() { _advertised = true; } - /** - * Try to change the size of the queue. This can only be done as long as nobody published yet. - * This is the case, for example when orb_subscribe was called before an orb_advertise. - * The queue size can only be increased. - * @param queue_size new size of the queue - * @return PX4_OK if queue size successfully set - */ - int update_queue_size(unsigned int queue_size); - /** * Print statistics * @param max_topic_length max topic name length for printing @@ -195,7 +186,7 @@ public: */ bool print_statistics(int max_topic_length); - uint8_t get_queue_size() const { return _queue_size; } + uint8_t get_queue_size() const { return _meta->o_queue; } int8_t subscriber_count() const { return _subscriber_count; } @@ -234,7 +225,7 @@ public: bool copy(void *dst, unsigned &generation) { if ((dst != nullptr) && (_data != nullptr)) { - if (_queue_size == 1) { + if (_meta->o_queue == 1) { ATOMIC_ENTER; memcpy(dst, _data, _meta->o_size); generation = _generation.load(); @@ -253,12 +244,12 @@ public: } // Compatible with normal and overflow conditions - if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) { + if (!is_in_range(current_generation - _meta->o_queue, generation, current_generation - 1)) { // Reader is too far behind: some messages are lost - generation = current_generation - _queue_size; + generation = current_generation - _meta->o_queue; } - memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size); + memcpy(dst, _data + (_meta->o_size * (generation % _meta->o_queue)), _meta->o_size); ATOMIC_LEAVE; ++generation; @@ -295,7 +286,7 @@ private: const uint8_t _instance; /**< orb multi instance identifier */ bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */ - uint8_t _queue_size; /**< maximum number of elements in the queue */ + int8_t _subscriber_count{0}; diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 5b5530694b..7e2fe4461a 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -265,8 +265,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) return ret; } -orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) { #ifdef ORB_USE_PUBLISHER_RULES @@ -300,19 +299,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, return nullptr; } - /* Set the queue size. This must be done before the first publication; thus it fails if - * this is not the first advertiser. - */ - int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); - - if (result < 0 && queue_size > 1) { - PX4_WARN("orb_advertise_multi: failed to set queue size"); - } - /* get the advertiser handle and close the node */ orb_advert_t advertiser; - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); if (result == PX4_ERROR) { @@ -602,6 +592,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 +619,7 @@ 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->mark_as_advertised(); _remote_topics.insert(topic_name); return 0; @@ -622,27 +628,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 +651,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 { diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 1c2870b1ee..d5202b9236 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -215,17 +215,15 @@ public: * @param data A pointer to the initial data to be published. * For topics updated by interrupt handlers, the advertisement * must be performed from non-interrupt context. - * @param queue_size Maximum number of buffered elements. If this is 1, no queuing is - * used. * @return nullptr on error, otherwise returns an object pointer * that can be used to publish to the topic. * If the topic in question is not known (due to an * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return nullptr and set errno to ENOENT. */ - orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1) + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data = nullptr) { - return orb_advertise_multi(meta, data, nullptr, queue_size); + return orb_advertise_multi(meta, data, nullptr); } /** @@ -250,16 +248,13 @@ public: * @param instance Pointer to an integer which will yield the instance ID (0-based) * of the publication. This is an output parameter and will be set to the newly * created instance, ie. 0 for the first advertiser, 1 for the next and so on. - * @param queue_size Maximum number of buffered elements. If this is 1, no queuing is - * used. * @return nullptr on error, otherwise returns a handle * that can be used to publish to the topic. * If the topic in question is not known (due to an * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return nullptr and set errno to ENOENT. */ - orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size = 1); + orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance); /** * Unadvertise a topic. diff --git a/platforms/common/uORB/uORBManagerUsr.cpp b/platforms/common/uORB/uORBManagerUsr.cpp index af00dbb9ba..e1a413e4c6 100644 --- a/platforms/common/uORB/uORBManagerUsr.cpp +++ b/platforms/common/uORB/uORBManagerUsr.cpp @@ -89,8 +89,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) return data.ret; } -orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) { /* open the node as an advertiser */ int fd = node_open(meta, true, instance); @@ -100,19 +99,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, return nullptr; } - /* Set the queue size. This must be done before the first publication; thus it fails if - * this is not the first advertiser. - */ - int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); - - if (result < 0 && queue_size > 1) { - PX4_WARN("orb_advertise_multi: failed to set queue size"); - } - /* get the advertiser handle and close the node */ orb_advert_t advertiser; - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); if (result == PX4_ERROR) { diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp index f8631fa04d..b40b60ad47 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -574,8 +574,8 @@ int uORBTest::UnitTest::test_wrap_around() bool updated{false}; // Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation - const int queue_size = 16; - ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_wrap_around), nullptr, queue_size); + const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_wrap_around)); + ptopic = orb_advertise(ORB_ID(orb_test_medium_wrap_around), nullptr); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); @@ -828,9 +828,9 @@ int uORBTest::UnitTest::test_queue() return test_fail("subscribe failed: %d", errno); } - const int queue_size = 16; + const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue)); orb_test_medium_s t{}; - ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); + ptopic = orb_advertise(ORB_ID(orb_test_medium_queue), &t); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); @@ -935,9 +935,9 @@ int uORBTest::UnitTest::pub_test_queue_main() { orb_test_medium_s t{}; orb_advert_t ptopic{nullptr}; - const int queue_size = 50; + const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue_poll)); - if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) { + if ((ptopic = orb_advertise(ORB_ID(orb_test_medium_queue_poll), &t)) == nullptr) { _thread_should_exit = true; return test_fail("advertise failed: %d", errno); } diff --git a/platforms/ros2/include/uORB/Publication.hpp b/platforms/ros2/include/uORB/Publication.hpp index 3b53424e0b..b7f1d0768d 100644 --- a/platforms/ros2/include/uORB/Publication.hpp +++ b/platforms/ros2/include/uORB/Publication.hpp @@ -45,28 +45,10 @@ namespace uORB { -template class DefaultQueueSize -{ -private: - template - static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *) - { - return T::ORB_QUEUE_LENGTH; - } - - template static constexpr uint8_t get_queue_size(...) - { - return 1; - } - -public: - static constexpr unsigned value = get_queue_size(nullptr); -}; - /** * uORB publication wrapper class */ -template::value> +template class Publication { public: diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index c03e63aca4..4f49e5692b 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -314,7 +314,7 @@ CameraTrigger::CameraTrigger() : // Advertise critical publishers here, because we cannot advertise in interrupt context camera_trigger_s trigger{}; - _trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH); + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); } CameraTrigger::~CameraTrigger() diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 042dbe2097..42f361d439 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -64,13 +64,10 @@ /** Get the priority for the topic */ #define ORBIOCGPRIORITY _ORBIOC(14) -/** Set the queue size of the topic */ -#define ORBIOCSETQUEUESIZE _ORBIOC(15) - /** Get the minimum interval at which the topic can be seen to be updated for this subscription */ -#define ORBIOCGETINTERVAL _ORBIOC(16) +#define ORBIOCGETINTERVAL _ORBIOC(15) /** Check whether the topic is advertised, sets *(unsigned long *)arg to 1 if advertised, 0 otherwise */ -#define ORBIOCISADVERTISED _ORBIOC(17) +#define ORBIOCISADVERTISED _ORBIOC(16) #endif /* _DRV_UORB_H */ diff --git a/src/drivers/tone_alarm/ToneAlarm.cpp b/src/drivers/tone_alarm/ToneAlarm.cpp index 5893f9018f..256f218b76 100644 --- a/src/drivers/tone_alarm/ToneAlarm.cpp +++ b/src/drivers/tone_alarm/ToneAlarm.cpp @@ -45,8 +45,6 @@ using namespace time_literals; ToneAlarm::ToneAlarm() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { - // ensure ORB_ID(tune_control) is advertised with correct queue depth - orb_advertise_queue(ORB_ID(tune_control), nullptr, tune_control_s::ORB_QUEUE_LENGTH); } ToneAlarm::~ToneAlarm() diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index 84ed38287d..b00d04f4db 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -151,7 +151,7 @@ private: transponder_report_s tr{}; - orb_advert_t fake_traffic_report_publisher = orb_advertise_queue(ORB_ID(transponder_report), &tr, (unsigned int)20); + orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr); TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT}; diff --git a/src/lib/events/events.cpp b/src/lib/events/events.cpp index c4d6867020..9bb215d356 100644 --- a/src/lib/events/events.cpp +++ b/src/lib/events/events.cpp @@ -61,7 +61,7 @@ void send(EventType &event) orb_publish(ORB_ID(event), orb_event_pub, &event); } else { - orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH); + orb_event_pub = orb_advertise(ORB_ID(event), &event); } pthread_mutex_unlock(&publish_event_mutex); diff --git a/src/lib/systemlib/mavlink_log.cpp b/src/lib/systemlib/mavlink_log.cpp index d31136f215..94870af7ae 100644 --- a/src/lib/systemlib/mavlink_log.cpp +++ b/src/lib/systemlib/mavlink_log.cpp @@ -73,6 +73,6 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg); } else { - *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH); + *mavlink_log_pub = orb_advertise(ORB_ID(mavlink_log), &log_msg); } } diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp index bc919080bf..912d3bb129 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp @@ -55,7 +55,7 @@ public: void SetUp() override { // ensure topic exists, otherwise we might lose first queued events - orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH); + orb_advertise(ORB_ID(event), nullptr); } }; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index f642541fb7..48c9398249 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -153,7 +153,7 @@ int buzzer_init() tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW] = 800000; tune_durations[tune_control_s::TUNE_ID_SINGLE_BEEP] = 300000; - tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH); + tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control); return PX4_OK; } @@ -330,7 +330,7 @@ int led_init() led_control.mode = led_control_s::MODE_OFF; led_control.priority = 0; led_control.timestamp = hrt_absolute_time(); - led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH); + led_control_pub = orb_advertise(ORB_ID(led_control), &led_control); /* first open normal LEDs */ fd_leds = px4_open(LED0_DEVICE_PATH, O_RDWR); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb26398ba9..2467554e97 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -124,13 +124,13 @@ Mavlink::Mavlink() : // ensure topic exists, otherwise we might lose first queued commands if (orb_exists(ORB_ID(vehicle_command), 0) == PX4_ERROR) { - orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH); + orb_advertise(ORB_ID(vehicle_command), nullptr); } _vehicle_command_sub.subscribe(); if (orb_exists(ORB_ID(event), 0) == PX4_ERROR) { - orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH); + orb_advertise(ORB_ID(event), nullptr); } _event_sub.subscribe(); diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index c664d87a9f..cade47b5fd 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -168,7 +168,7 @@ bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id { @[ for idx, sub in enumerate(subscriptions + subscriptions_multi)]@ { - uint16_t queue_depth = uORB::DefaultQueueSize<@(sub['simple_base_type'])_s>::value * 2; // use a bit larger queue size than internal + uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['simple_base_type']))) * 2; // use a bit larger queue size than internal create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])", queue_depth); } @[ end for]@ diff --git a/src/modules/uxrce_dds_client/vehicle_command_srv.cpp b/src/modules/uxrce_dds_client/vehicle_command_srv.cpp index aac0bd2c58..c25d6757d4 100644 --- a/src/modules/uxrce_dds_client/vehicle_command_srv.cpp +++ b/src/modules/uxrce_dds_client/vehicle_command_srv.cpp @@ -39,8 +39,7 @@ VehicleCommandSrv::VehicleCommandSrv(uxrSession *session, uxrStreamId reliable_o uxrStreamId input_stream_id, uxrObjectId participant_id, const char *client_namespace, const uint8_t index) : SrvBase(session, reliable_out_stream_id, input_stream_id, participant_id) { - uint16_t queue_depth = uORB::DefaultQueueSize::value * - 2; // use a bit larger queue size than internal + uint16_t queue_depth = orb_get_queue_size(ORB_ID(vehicle_command)) * 2; // use a bit larger queue size than internal create_replier(input_stream_id, participant_id, index, client_namespace, "vehicle_command", "VehicleCommand", queue_depth); }; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp index bd16b64398..550c1a3f45 100644 --- a/src/modules/zenoh/subscribers/uorb_subscriber.hpp +++ b/src/modules/zenoh/subscribers/uorb_subscriber.hpp @@ -55,7 +55,7 @@ public: _cdr_ops(ops) { int instance = 0; - _uorb_pub_handle = orb_advertise_multi_queue(_uorb_meta, nullptr, &instance, 1); //FIXME template magic qsize + _uorb_pub_handle = orb_advertise_multi(_uorb_meta, nullptr, &instance); }; ~uORB_Zenoh_Subscriber() override = default;