From 57df7e35b284ce49065e469c1bb828055a24b459 Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Fri, 8 Mar 2024 13:28:24 -0800 Subject: [PATCH] uORB: make queue size (ORB_QUEUE_LENGTH) completely static (#22815) Previously uORB queue size was an awkward mix of runtime configurable (at advertise or IOCTL before allocate), but effectively static with all queue size settings (outside of test code) actually coming from the topic declaration (presently ORB_QUEUE_LENGTH in the .msg). This change finally resolves the inconsistency making the queue size fully static. Additionally there were some corner cases that the muorb and orb communicator implementation were not correctly handling. This PR provides fixes for those issues. Also correctly sets remote queue lengths now based on the topic definitions. * Made setting of uORB topic queue size in based on topic definition only * Fixes to the ModalAI muorb implementation * Removed libfc sensor from format checks * msg/TransponderReport.msg ORB_QUEUE_LENGTH 8->16 (was set to higher in AdsbConflict.h --------- Co-authored-by: Eric Katzfey Co-authored-by: Daniel Agar --- Tools/astyle/files_to_check_code_style.sh | 1 + Tools/msg/templates/uorb/msg.cpp.em | 9 ++- msg/OrbTestMedium.msg | 2 + msg/TransponderReport.msg | 2 +- platforms/common/px4_log.cpp | 2 +- platforms/common/uORB/Publication.hpp | 22 +------ platforms/common/uORB/PublicationMulti.hpp | 4 +- platforms/common/uORB/uORB.cpp | 19 +++--- platforms/common/uORB/uORB.h | 38 ++++++----- platforms/common/uORB/uORBDeviceNode.cpp | 60 ++---------------- platforms/common/uORB/uORBDeviceNode.hpp | 23 +++---- platforms/common/uORB/uORBManager.cpp | 63 ++++++++----------- platforms/common/uORB/uORBManager.hpp | 11 +--- platforms/common/uORB/uORBManagerUsr.cpp | 14 +---- .../uORB/uORB_tests/uORBTest_UnitTest.cpp | 12 ++-- platforms/ros2/include/uORB/Publication.hpp | 20 +----- src/drivers/camera_trigger/camera_trigger.cpp | 2 +- src/drivers/drv_orb_dev.h | 7 +-- src/drivers/tone_alarm/ToneAlarm.cpp | 2 - src/lib/adsb/AdsbConflict.h | 2 +- src/lib/events/events.cpp | 2 +- src/lib/systemlib/mavlink_log.cpp | 2 +- .../HealthAndArmingChecksTest.cpp | 2 +- src/modules/commander/commander_helper.cpp | 4 +- src/modules/mavlink/mavlink_main.cpp | 4 +- src/modules/uxrce_dds_client/dds_topics.h.em | 2 +- .../uxrce_dds_client/vehicle_command_srv.cpp | 3 +- .../zenoh/subscribers/uorb_subscriber.hpp | 2 +- 28 files changed, 108 insertions(+), 228 deletions(-) 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;