diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index b792e0cf1d..251d2244b6 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -55,7 +55,7 @@ uORB::DeviceMaster::~DeviceMaster() } int -uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, int priority) +uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority) { int ret = PX4_ERROR; @@ -119,17 +119,36 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in delete node; if (ret == -EEXIST) { - /* if the node exists already, get the existing one and check if - * something has been published yet. */ + /* if the node exists already, get the existing one and check if it's advertised. */ uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries); - if (existing_node != nullptr && !existing_node->is_advertised()) { - /* nothing has been published yet, lets claim it */ - existing_node->set_priority(priority); + /* + * We can claim an existing node in these cases: + * - The node is not advertised (yet). It means there is already one or more subscribers or it was + * unadvertised. + * - We are a single-instance advertiser requesting the first instance. + * (Usually we don't end up here, but we might in case of a race condition between 2 + * advertisers). + * - We are a subscriber requesting a certain instance. + * (Also we usually don't end up in that case, but we might in case of a race condtion + * between an advertiser and subscriber). + */ + bool is_single_instance_advertiser = is_advertiser && !instance; + + if (existing_node != nullptr && + (!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) { + if (is_advertiser) { + existing_node->set_priority(priority); + /* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers + * could get the same instance). + */ + existing_node->mark_as_advertised(); + } + ret = PX4_OK; } else { - /* otherwise: data has already been published, keep looking */ + /* otherwise: already advertised, keep looking */ } } @@ -137,7 +156,11 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in free((void *)devpath); } else { - // add to the node map;. + if (is_advertiser) { + node->mark_as_advertised(); + } + + // add to the node map. _node_list.add(node); } diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index 60c9c0d628..4869bd6c21 100644 --- a/src/modules/uORB/uORBDeviceMaster.hpp +++ b/src/modules/uORB/uORBDeviceMaster.hpp @@ -60,7 +60,7 @@ class uORB::DeviceMaster { public: - int advertise(const struct orb_metadata *meta, int *instance, int priority); + int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority); /** * Public interface for getDeviceNodeLocked(). Takes care of synchronization. diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index b08627daa2..3b0b950641 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -89,6 +89,7 @@ uORB::DeviceNode::open(cdev::file_t *filp) ret = -EBUSY; } + mark_as_advertised(); unlock(); /* now complete the open */ @@ -275,7 +276,6 @@ 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 */ _generation++; - _advertised = true; ATOMIC_LEAVE; diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 03d5077b78..56fd181fd3 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -162,6 +162,8 @@ public: * and publish to this node or if another node should be tried. */ bool is_advertised() const { return _advertised; } + 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. diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 1d11513404..d83e75fa1b 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -316,14 +316,12 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval) return ret; } -int uORB::Manager::node_advertise(const struct orb_metadata *meta, int *instance, int priority) +int uORB::Manager::node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority) { int ret = PX4_ERROR; - /* fill advertiser data */ - if (get_device_master()) { - ret = _device_master->advertise(meta, instance, priority); + ret = _device_master->advertise(meta, is_advertiser, instance, priority); } /* it's PX4_OK if it already exists */ @@ -373,7 +371,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i if (fd < 0) { /* try to create the node */ - ret = node_advertise(meta, instance, priority); + ret = node_advertise(meta, advertiser, instance, priority); if (ret == PX4_OK) { /* update the path, as it might have been updated during the node_advertise call */ @@ -385,7 +383,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i } } - /* on success, try the open again */ + /* on success, try to open again */ if (ret == PX4_OK) { fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); } diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 42d195b26f..70c912bc3d 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -386,11 +386,9 @@ private: // class methods /** * Advertise a node; don't consider it an error if the node has * already been advertised. - * - * @todo verify that the existing node is the same as the one - * we tried to advertise. */ - int node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT); + int node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance = nullptr, + int priority = ORB_PRIO_DEFAULT); /** * Common implementation for orb_advertise and orb_subscribe.