uorb: add uorb_unadvertise method

This is necessary when using multiple instances of a topic. However it does
not free the underlying resources, as it is assumed they will be used again
at a later point.

src/modules/uORB/Publication.cpp

@@ -96,6 +96,7 @@ void PublicationBase::update(void *data)
 
 PublicationBase::~PublicationBase()
 {
+	orb_unadvertise(getHandle());
 }
 
 PublicationNode::PublicationNode(const struct orb_metadata *meta,

src/modules/uORB/uORB.cpp

@@ -51,6 +51,11 @@ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *da
 	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority);
 }
 
+int orb_unadvertise(orb_advert_t handle)
+{
+	return uORB::Manager::get_instance()->orb_unadvertise(handle);
+}
+
 int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance,
 		     int priority)
 {

src/modules/uORB/uORB.h

@@ -142,6 +142,11 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d
 extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
 					int priority) __EXPORT;
 
+/**
+ * @see uORB::Manager::orb_unadvertise()
+ */
+extern int orb_unadvertise(orb_advert_t handle) __EXPORT;
+
 /**
  * Advertise as the publisher of a topic.
  *

src/modules/uORB/uORBDevices_nuttx.cpp

@@ -336,6 +336,30 @@ uORB::DeviceNode::publish
 	return OK;
 }
 
+int uORB::DeviceNode::unadvertise(orb_advert_t handle)
+{
+	if (handle == nullptr) {
+		return -EINVAL;
+	}
+
+	uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
+
+	/*
+	 * We are cheating a bit here. First, with the current implementation, we can only
+	 * have multiple publishers for instance 0. In this case the caller will have
+	 * instance=nullptr and _published has no effect at all. Thus no unadvertise is
+	 * necessary.
+	 * In case of multiple instances, we have at most 1 publisher per instance and
+	 * we can signal an instance as 'free' by setting _published to false.
+	 * We never really free the DeviceNode, for this we would need reference counting
+	 * of subscribers and publishers. But we also do not have a leak since future
+	 * publishers reuse the same DeviceNode object.
+	 */
+	devnode->_published = false;
+
+	return PX4_OK;
+}
+
 pollevent_t
 uORB::DeviceNode::poll_state(struct file *filp)
 {

src/modules/uORB/uORBDevices_nuttx.hpp

@@ -122,6 +122,8 @@ public:
 		const void *data
 	);
 
+	static int        unadvertise(orb_advert_t handle);
+
 	/**
 	 * processes a request for add subscription from remote
 	 * @param rateInHz
@@ -184,7 +186,8 @@ private:
 	uint8_t     *_data;   /**< allocated object buffer */
 	hrt_abstime   _last_update; /**< time the object was last updated */
 	volatile unsigned   _generation;  /**< object generation count */
-	pid_t     _publisher; /**< if nonzero, current publisher */
+	pid_t     _publisher; /**< if nonzero, current publisher. Only used inside the advertise call.
+					We allow one publisher to have an open file descriptor at the same time. */
 	const int   _priority;  /**< priority of topic */
 	bool _published;  /**< has ever data been published */
 

src/modules/uORB/uORBDevices_posix.cpp

@@ -352,6 +352,30 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
 	return PX4_OK;
 }
 
+int uORB::DeviceNode::unadvertise(orb_advert_t handle)
+{
+	if (handle == nullptr) {
+		return -EINVAL;
+	}
+
+	uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
+
+	/*
+	 * We are cheating a bit here. First, with the current implementation, we can only
+	 * have multiple publishers for instance 0. In this case the caller will have
+	 * instance=nullptr and _published has no effect at all. Thus no unadvertise is
+	 * necessary.
+	 * In case of multiple instances, we have at most 1 publisher per instance and
+	 * we can signal an instance as 'free' by setting _published to false.
+	 * We never really free the DeviceNode, for this we would need reference counting
+	 * of subscribers and publishers. But we also do not have a leak since future
+	 * publishers reuse the same DeviceNode object.
+	 */
+	devnode->_published = false;
+
+	return PX4_OK;
+}
+
 pollevent_t
 uORB::DeviceNode::poll_state(device::file_t *filp)
 {

src/modules/uORB/uORBDevices_posix.hpp

@@ -59,6 +59,8 @@ public:
 
 	static ssize_t    publish(const orb_metadata *meta, orb_advert_t handle, const void *data);
 
+	static int        unadvertise(orb_advert_t handle);
+
 	/**
 	 * processes a request for add subscription from remote
 	 * @param rateInHz
@@ -122,7 +124,8 @@ private:
 	uint8_t     *_data;   /**< allocated object buffer */
 	hrt_abstime   _last_update; /**< time the object was last updated */
 	volatile unsigned   _generation;  /**< object generation count */
-	unsigned long     _publisher; /**< if nonzero, current publisher */
+	unsigned long     _publisher; /**< if nonzero, current publisher. Only used inside the advertise call.
+					We allow one publisher to have an open file descriptor at the same time. */
 	const int   _priority;  /**< priority of topic */
 	bool _published;  /**< has ever data been published */
 

src/modules/uORB/uORBManager.cpp

@@ -129,6 +129,11 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 	return advertiser;
 }
 
+int uORB::Manager::orb_unadvertise(orb_advert_t handle)
+{
+	return uORB::DeviceNode::unadvertise(handle);
+}
+
 int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
 {
 	return node_open(PUBSUB, meta, nullptr, false);

src/modules/uORB/uORBManager.hpp

@@ -130,6 +130,14 @@ public:
 					 int priority) ;
 
 
+	/**
+	 * Unadvertise a topic.
+	 *
+	 * @param handle  handle returned by orb_advertise or orb_advertise_multi.
+	 * @return 0 on success
+	 */
+	int orb_unadvertise(orb_advert_t handle);
+
 	/**
 	 * Publish new data to a topic.
 	 *