uORBDevices_posix: copied over fixes from uORBDevices_nuttx

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);
 

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);