From 21d9bd70a3f41fa77c6c81c584a64e716f849dda Mon Sep 17 00:00:00 2001
From: Ban Siesta <bansiesta@gmail.com>
Date: Sat, 13 Jun 2015 15:06:16 +0100
Subject: [PATCH] uORBDevices_nuttx: Fixed the bug that a publisher had to be
 started before an advertiser for multi_pub/subs.

This is achieved using
- A "published"  flag for each uORB device node
- A check before increasing the instance count (basically, a node is
  re-used if nothing has been published on it before which means that it
  has been created by a subscriber.
---
 src/modules/uORB/uORBDevices_nuttx.cpp | 27 +++++++++++++++++++++++++-
 src/modules/uORB/uORBDevices_nuttx.hpp |  8 ++++++++
 2 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp
index 0b3ee4ded5..5d58dbcae9 100644
--- a/src/modules/uORB/uORBDevices_nuttx.cpp
+++ b/src/modules/uORB/uORBDevices_nuttx.cpp
@@ -59,6 +59,7 @@ uORB::DeviceNode::DeviceNode
 	_generation(0),
 	_publisher(0),
 	_priority(priority),
+	_published(false),
 	_IsRemoteSubscriberPresent(false),
 	_subscriber_count(0)
 {
@@ -254,6 +255,8 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen)
 	/* notify any poll waiters */
 	poll_notify(POLLIN);
 
+	_published = true;
+
 	return _meta->o_size;
 }
 
@@ -483,6 +486,14 @@ void uORB::DeviceNode::remove_internal_subscriber()
 	}
 }
 
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+bool uORB::DeviceNode::is_published()
+{
+	return _published;
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)
@@ -616,9 +627,23 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* initialise the node - this may fail if e.g. a node with this name already exists */
 				ret = node->init();
 
-				/* if init failed, discard the node and its name */
 				if (ret != OK) {
+					/* if init failed, discard the node */
 					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 = 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_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp
index 4eb5f83101..2c9ea26719 100644
--- a/src/modules/uORB/uORBDevices_nuttx.hpp
+++ b/src/modules/uORB/uORBDevices_nuttx.hpp
@@ -159,6 +159,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(struct file *filp);
 	virtual void poll_notify_one(struct pollfd *fds, pollevent_t events);
@@ -179,6 +186,7 @@ private:
 	volatile unsigned   _generation;  /**< object generation count */
 	pid_t     _publisher; /**< if nonzero, current publisher */
 	const int   _priority;  /**< priority of topic */
+	bool _published;  /**< has ever data been published */
 
 private: // private class methods.