diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 2fce0e8efb..f53867a083 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -68,6 +68,7 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, _generation(0), _publisher(0), _priority(priority), + _published(false), _subscriber_count(0) { // enable debug() calls @@ -264,6 +265,8 @@ uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen) /* notify any poll waiters */ poll_notify(POLLIN); + _published = true; + return _meta->o_size; } @@ -500,6 +503,14 @@ void uORB::DeviceNode::remove_internal_subscriber() } } + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +bool uORB::DeviceNode::is_published() +{ + return _published; +} + //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz) @@ -640,6 +651,20 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* if init failed, discard the node and its name */ if (ret != PX4_OK) { delete node; + + if (ret == -EEXIST) { + /* if the node exists already, get the existing one and check if + * something has been published yet. */ + uORB::DeviceNode *existing_node = GetDeviceNode(devpath); + + if ((existing_node != nullptr) && !(existing_node->is_published())) { + /* nothing has been published yet, lets claim it */ + ret = PX4_OK; + } else { + /* otherwise: data has already been published, keep looking */ + } + } + /* also discard the name now */ free((void *)objname); free((void *)devpath); diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index 1ac0897381..4d45ea8431 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -96,6 +96,13 @@ public: */ void remove_internal_subscriber(); + /** + * Return true if this topic has been published. + * + * This is used in the case of multi_pub/sub to check if it's valid to advertise + * and publish to this node or if another node should be tried. */ + bool is_published(); + protected: virtual pollevent_t poll_state(device::file_t *filp); virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); @@ -116,6 +123,7 @@ private: volatile unsigned _generation; /**< object generation count */ unsigned long _publisher; /**< if nonzero, current publisher */ const int _priority; /**< priority of topic */ + bool _published; /**< has ever data been published */ SubscriberData *filp_to_sd(device::file_t *filp);