uORB add WorkItem callbacks on publication

src/modules/uORB/uORB.cpp

@@ -107,6 +107,16 @@ int  orb_exists(const struct orb_metadata *meta, int instance)
 	return uORB::Manager::get_instance()->orb_exists(meta, instance);
 }
 
+int orb_register_work_callback(px4::WorkItem *item, const orb_metadata *meta, int instance)
+{
+	return uORB::Manager::get_instance()->orb_register_work_callback(item, meta, instance);
+}
+
+int orb_unregister_work_callback(px4::WorkItem *item, const orb_metadata *meta, int instance)
+{
+	return uORB::Manager::get_instance()->orb_unregister_work_callback(item, meta, instance);
+}
+
 int  orb_group_count(const struct orb_metadata *meta)
 {
 	unsigned instance = 0;

src/modules/uORB/uORB.h

@@ -43,6 +43,12 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#ifdef __cplusplus
+namespace px4
+{
+class WorkItem; // forward declaration
+} // namespace px4
+#endif
 
 /**
  * Object metadata.
@@ -250,6 +256,22 @@ extern int	orb_set_interval(int handle, unsigned interval) __EXPORT;
  */
 extern int	orb_get_interval(int handle, unsigned *interval) __EXPORT;
 
+#ifdef __cplusplus
+/**
+ * @see uORB::Manager::orb_register_work_callback()
+ */
+extern int
+orb_register_work_callback(px4::WorkItem *item, const orb_metadata *meta, int instance = 0) __EXPORT;
+
+/**
+ * @see uORB::Manager::orb_unregister_work_callback()
+ */
+
+extern int
+orb_unregister_work_callback(px4::WorkItem *item, const orb_metadata *meta, int instance = 0) __EXPORT;
+
+#endif /* __cplusplus */
+
 __END_DECLS
 
 /* Diverse uORB header defines */ //XXX: move to better location

src/modules/uORB/uORBDeviceNode.cpp

@@ -317,6 +317,11 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 
 	_published = true;
 
+	// schedule registered work items
+	for (auto item : _registered_work_items) {
+		item->ScheduleNow();
+	}
+
 	ATOMIC_LEAVE;
 
 	/* notify any poll waiters */
@@ -673,3 +678,40 @@ int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
 	_queue_size = queue_size;
 	return PX4_OK;
 }
+
+bool
+uORB::DeviceNode::register_work_item(px4::WorkItem *item)
+{
+
+	if (item != nullptr) {
+		ATOMIC_ENTER;
+
+		// prevent duplicate registrations
+		for (auto existing_item : _registered_work_items) {
+			if (item == existing_item) {
+				PX4_ERR("work item already registered, skipping");
+				ATOMIC_LEAVE;
+				return false;
+			}
+		}
+
+		_registered_work_items.add(item);
+		ATOMIC_LEAVE;
+		return true;
+	}
+
+	return false;
+}
+
+bool
+uORB::DeviceNode::unregister_work_item(px4::WorkItem *item)
+{
+	if (item != nullptr) {
+		ATOMIC_ENTER;
+		_registered_work_items.remove(item);
+		ATOMIC_LEAVE;
+		return true;
+	}
+
+	return false;
+}

src/modules/uORB/uORBDeviceNode.hpp

@@ -39,6 +39,7 @@
 #include <lib/cdev/CDev.hpp>
 
 #include <containers/List.hpp>
+#include <px4_work_queue/WorkItem.hpp>
 
 namespace uORB
 {
@@ -231,6 +232,12 @@ class uORB::DeviceNode : public cdev::CDev, public ListNode<uORB::DeviceNode *>
 	 */
 	uint64_t copy_and_get_timestamp(void *dst, unsigned &generation);
 
+	// add item to list of work items to schedule on node update
+	bool register_work_item(px4::WorkItem *item);
+
+	// remove item from list of work items
+	bool unregister_work_item(px4::WorkItem *item);
+
 protected:
 
 	pollevent_t poll_state(cdev::file_t *filp) override;
@@ -269,6 +276,7 @@ class uORB::DeviceNode : public cdev::CDev, public ListNode<uORB::DeviceNode *>
 	uint8_t     *_data{nullptr};   /**< allocated object buffer */
 	hrt_abstime   _last_update{0}; /**< time the object was last updated */
 	volatile unsigned   _generation{0};  /**< object generation count */
+	List<px4::WorkItem *>	_registered_work_items;
 	uint8_t   _priority;  /**< priority of the topic */
 	bool _published{false};  /**< has ever data been published */
 	uint8_t _queue_size; /**< maximum number of elements in the queue */

src/modules/uORB/uORBManager.cpp

@@ -44,6 +44,8 @@
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"
 
+#include <px4_work_queue/WorkItem.hpp>
+
 uORB::Manager *uORB::Manager::_Instance = nullptr;
 
 bool uORB::Manager::initialize()
@@ -316,6 +318,44 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
 	return ret;
 }
 
+int
+uORB::Manager::orb_register_work_callback(px4::WorkItem *item, const orb_metadata *meta, int instance)
+{
+	// find node and insert callback (like pollset)
+	if ((item != nullptr) && (meta != nullptr)) {
+		if (get_device_master()) {
+			uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance);
+
+			if (node != nullptr) {
+				if (node->register_work_item(item)) {
+					return PX4_OK;
+				}
+			}
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+int
+uORB::Manager::orb_unregister_work_callback(px4::WorkItem *item, const orb_metadata *meta, int instance)
+{
+	// find node in list and remove
+	if ((item != nullptr) && (meta != nullptr)) {
+		if (get_device_master()) {
+			uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance);
+
+			if (node != nullptr) {
+				if (node->unregister_work_item(item)) {
+					return PX4_OK;
+				}
+			}
+		}
+	}
+
+	return PX4_ERROR;
+}
+
 int uORB::Manager::node_advertise(const struct orb_metadata *meta, int *instance, int priority)
 {
 	int ret = PX4_ERROR;

src/modules/uORB/uORBManager.hpp

@@ -51,6 +51,8 @@
 #include "uORBCommunicator.hpp"
 #endif /* ORB_COMMUNICATOR */
 
+#include <px4_work_queue/WorkItem.hpp>
+
 namespace uORB
 {
 class Manager;
@@ -347,7 +349,6 @@ class uORB::Manager
 	 */
 	int  orb_set_interval(int handle, unsigned interval);
 
-
 	/**
 	 * Get the minimum interval between which updates are seen for a subscription.
 	 *
@@ -359,6 +360,26 @@ class uORB::Manager
 	 */
 	int	orb_get_interval(int handle, unsigned *interval);
 
+	/**
+	 * Register work item callback on orb publish
+	 *
+	 * @param item   Valid WorkItem to schedule on new publication
+	 * @param meta    ORB topic metadata.
+	 * @param instance  ORB instance
+	 * @return    OK if the item was registered successfully, PX4_ERROR otherwise.
+	 */
+	int  orb_register_work_callback(px4::WorkItem *item, const orb_metadata *meta, int instance = 0);
+
+	/**
+	 * Unregister work item callback on orb publish
+	 *
+	 * @param item   Valid WorkItem to schedule on new publication
+	 * @param meta    ORB topic metadata.
+	 * @param instance  ORB instance
+	 * @return    OK if the item was unregistered successfully, PX4_ERROR otherwise.
+	 */
+	int  orb_unregister_work_callback(px4::WorkItem *item, const orb_metadata *meta, int instance = 0);
+
 #ifdef ORB_COMMUNICATOR
 	/**
 	 * Method to set the uORBCommunicator::IChannel instance.

src/platforms/common/px4_work_queue/WorkItem.hpp

@@ -38,13 +38,14 @@
 #include "WorkQueue.hpp"
 
 #include <containers/IntrusiveQueue.hpp>
+#include <containers/List.hpp>
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 
 namespace px4
 {
 
-class WorkItem : public IntrusiveQueueNode<WorkItem *>
+class WorkItem : public IntrusiveQueueNode<WorkItem *>, public ListNode<WorkItem *>
 {
 public: