From 63db2658e5201c6e2600dfded9f2efbf5067333b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 3 Jun 2019 12:14:01 -0400 Subject: [PATCH] uORB add WorkItem callbacks on publication --- src/modules/uORB/uORB.cpp | 10 +++++ src/modules/uORB/uORB.h | 22 ++++++++++ src/modules/uORB/uORBDeviceNode.cpp | 42 +++++++++++++++++++ src/modules/uORB/uORBDeviceNode.hpp | 8 ++++ src/modules/uORB/uORBManager.cpp | 40 ++++++++++++++++++ src/modules/uORB/uORBManager.hpp | 23 +++++++++- .../common/px4_work_queue/WorkItem.hpp | 3 +- 7 files changed, 146 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index f962ed3e5d4d..e17a869c6c11 100644 --- a/src/modules/uORB/uORB.cpp +++ b/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; diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index a75dfb1f074d..7682a96c663a 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -43,6 +43,12 @@ #include #include +#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 diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index 484ae4ab2459..dd4e3ea3550b 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/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; +} diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index ee0ec759dc1e..7bf88463df1d 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -39,6 +39,7 @@ #include #include +#include namespace uORB { @@ -231,6 +232,12 @@ class uORB::DeviceNode : public cdev::CDev, public ListNode */ 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 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 _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 */ diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 20c9bf843298..4a14e1c15892 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -44,6 +44,8 @@ #include "uORBUtils.hpp" #include "uORBManager.hpp" +#include + 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; diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 42d195b26fe2..cfa1a163f56d 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -51,6 +51,8 @@ #include "uORBCommunicator.hpp" #endif /* ORB_COMMUNICATOR */ +#include + 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. diff --git a/src/platforms/common/px4_work_queue/WorkItem.hpp b/src/platforms/common/px4_work_queue/WorkItem.hpp index a71cd05c3848..bec81fbadf96 100644 --- a/src/platforms/common/px4_work_queue/WorkItem.hpp +++ b/src/platforms/common/px4_work_queue/WorkItem.hpp @@ -38,13 +38,14 @@ #include "WorkQueue.hpp" #include +#include #include #include namespace px4 { -class WorkItem : public IntrusiveQueueNode +class WorkItem : public IntrusiveQueueNode, public ListNode { public: