diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp index bb95f21334..7547e3b7fa 100644 --- a/platforms/common/uORB/Publication.hpp +++ b/platforms/common/uORB/Publication.hpp @@ -42,7 +42,7 @@ #include #include -#include "uORBDeviceNode.hpp" +#include "uORBManager.hpp" #include namespace uORB @@ -72,7 +72,7 @@ public: bool advertised() const { return _handle != nullptr; } - bool unadvertise() { return (DeviceNode::unadvertise(_handle) == PX4_OK); } + bool unadvertise() { return (Manager::orb_unadvertise(_handle) == PX4_OK); } orb_id_t get_topic() const { return get_orb_meta(_orb_id); } @@ -84,7 +84,7 @@ protected: { if (_handle != nullptr) { // don't automatically unadvertise queued publications (eg vehicle_command) - if (static_cast(_handle)->get_queue_size() == 1) { + if (Manager::orb_get_queue_size(_handle) == 1) { unadvertise(); } } @@ -129,7 +129,7 @@ public: advertise(); } - return (DeviceNode::publish(get_topic(), _handle, &data) == PX4_OK); + return (Manager::orb_publish(get_topic(), _handle, &data) == PX4_OK); } }; diff --git a/platforms/common/uORB/PublicationMulti.hpp b/platforms/common/uORB/PublicationMulti.hpp index 7a91bc907e..bc275940b6 100644 --- a/platforms/common/uORB/PublicationMulti.hpp +++ b/platforms/common/uORB/PublicationMulti.hpp @@ -96,7 +96,7 @@ public: { // advertise if not already advertised if (advertise()) { - return static_cast(_handle)->get_instance(); + return Manager::orb_get_instance(_handle); } return -1; diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 276e4ac8fa..9c17cab378 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -49,25 +49,14 @@ bool Subscription::subscribe() return true; } - if ((_orb_id != ORB_ID::INVALID) && uORB::Manager::get_instance()) { - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + if (_orb_id != ORB_ID::INVALID && uORB::Manager::get_instance()) { + unsigned initial_generation; + void *node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &initial_generation); - if (device_master != nullptr) { - - if (!device_master->deviceNodeExists(_orb_id, _instance)) { - return false; - } - - uORB::DeviceNode *node = device_master->getDeviceNode(get_topic(), _instance); - - if (node != nullptr) { - _node = node; - _node->add_internal_subscriber(); - - _last_generation = _node->get_initial_generation(); - - return true; - } + if (node) { + _node = node; + _last_generation = initial_generation; + return true; } } @@ -77,7 +66,7 @@ bool Subscription::subscribe() void Subscription::unsubscribe() { if (_node != nullptr) { - _node->remove_internal_subscriber(); + uORB::Manager::orb_remove_internal_subscriber(_node); } _node = nullptr; @@ -87,13 +76,7 @@ void Subscription::unsubscribe() bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); - - if (device_master != nullptr) { - if (!device_master->deviceNodeExists(_orb_id, instance)) { - return false; - } - + if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) { // if desired new instance exists, unsubscribe from current unsubscribe(); _instance = instance; diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index 8e082d2a60..9fcad62179 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -44,7 +44,6 @@ #include #include -#include "uORBDeviceNode.hpp" #include "uORBManager.hpp" #include "uORBUtils.hpp" @@ -120,14 +119,14 @@ public: bool advertised() { if (valid()) { - return _node->is_advertised(); + return Manager::is_advertised(_node); } // try to initialize if (subscribe()) { // check again if valid if (valid()) { - return _node->is_advertised(); + return Manager::is_advertised(_node); } } @@ -137,19 +136,19 @@ public: /** * Check if there is a new update. */ - bool updated() { return advertised() && _node->updates_available(_last_generation); } + bool updated() { return advertised() && Manager::updates_available(_node, _last_generation); } /** * Update the struct * @param dst The uORB message struct we are updating. */ - bool update(void *dst) { return updated() && _node->copy(dst, _last_generation); } + bool update(void *dst) { return updated() && Manager::orb_data_copy(_node, dst, _last_generation); } /** * Copy the struct * @param dst The uORB message struct we are updating. */ - bool copy(void *dst) { return advertised() && _node->copy(dst, _last_generation); } + bool copy(void *dst) { return advertised() && Manager::orb_data_copy(_node, dst, _last_generation); } /** * Change subscription instance @@ -166,9 +165,9 @@ protected: friend class SubscriptionCallback; friend class SubscriptionCallbackWorkItem; - DeviceNode *get_node() { return _node; } + void *get_node() { return _node; } - DeviceNode *_node{nullptr}; + void *_node{nullptr}; unsigned _last_generation{0}; /**< last generation the subscriber has seen */ diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp index 7f66d2396b..942a9fbe17 100644 --- a/platforms/common/uORB/SubscriptionCallback.hpp +++ b/platforms/common/uORB/SubscriptionCallback.hpp @@ -69,7 +69,7 @@ public: bool registerCallback() { if (!_registered) { - if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) { + if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) { // registered _registered = true; @@ -79,7 +79,7 @@ public: // try to register callback again if (_subscription.subscribe()) { - if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) { + if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) { _registered = true; } } @@ -94,7 +94,7 @@ public: void unregisterCallback() { if (_subscription.get_node()) { - _subscription.get_node()->unregister_callback(this); + Manager::unregister_callback(_subscription.get_node(), this); } _registered = false; @@ -164,7 +164,7 @@ public: { // schedule immediately if updated (queue depth or subscription interval) if ((_required_updates == 0) - || (_subscription.get_node()->updates_available(_subscription.get_last_generation()) >= _required_updates)) { + || (Manager::updates_available(_subscription.get_node(), _subscription.get_last_generation()) >= _required_updates)) { if (updated()) { _work_item->ScheduleNow(); } diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 27ce994eb0..31efad523c 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -94,7 +94,6 @@ int uorb_top(char **topic_filter, int num_filters) return OK; } - orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { return uORB::Manager::get_instance()->orb_advertise(meta, data); @@ -121,42 +120,42 @@ int orb_unadvertise(orb_advert_t handle) return uORB::Manager::get_instance()->orb_unadvertise(handle); } -int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { return uORB::Manager::get_instance()->orb_publish(meta, handle, data); } -int orb_subscribe(const struct orb_metadata *meta) +int orb_subscribe(const struct orb_metadata *meta) { return uORB::Manager::get_instance()->orb_subscribe(meta); } -int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance); } -int orb_unsubscribe(int handle) +int orb_unsubscribe(int handle) { return uORB::Manager::get_instance()->orb_unsubscribe(handle); } -int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer); } -int orb_check(int handle, bool *updated) +int orb_check(int handle, bool *updated) { return uORB::Manager::get_instance()->orb_check(handle, updated); } -int orb_exists(const struct orb_metadata *meta, int instance) +int orb_exists(const struct orb_metadata *meta, int instance) { return uORB::Manager::get_instance()->orb_exists(meta, instance); } -int orb_group_count(const struct orb_metadata *meta) +int orb_group_count(const struct orb_metadata *meta) { unsigned instance = 0; diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 0493c68fd8..97f156d125 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -112,8 +112,10 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) return ret; } - if (get_device_master()) { - uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance); + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + + if (dev) { + uORB::DeviceNode *node = dev->getDeviceNode(meta, instance); if (node != nullptr) { if (node->is_advertised()) { @@ -319,6 +321,65 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval) return ret; } + +bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) +{ + DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + + return device_master != nullptr && + device_master->deviceNodeExists(orb_id, instance); +} + +void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) +{ + uORB::DeviceNode *node = nullptr; + DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + + if (device_master != nullptr) { + node = device_master->getDeviceNode(get_orb_meta(orb_id), instance); + + if (node) { + node->add_internal_subscriber(); + *initial_generation = node->get_initial_generation(); + } + } + + return node; +} + +void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) +{ + static_cast(node_handle)->remove_internal_subscriber(); +} + +uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast(node_handle)->get_queue_size(); } + +bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation) +{ + return static_cast(node_handle)->copy(dst, generation); +} + +// add item to list of work items to schedule on node update +bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + return static_cast(node_handle)->register_callback(callback_sub); +} + +// remove item from list of work items +void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + static_cast(node_handle)->unregister_callback(callback_sub); +} + +uint8_t uORB::Manager::orb_get_instance(const void *node_handle) +{ + if (node_handle) { + return static_cast(node_handle)->get_instance(); + } + + return -1; +} + int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) { char path[orb_maxpath]; diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index b6e0ae784f..647f10a3ed 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -34,6 +34,7 @@ #ifndef _uORBManager_hpp_ #define _uORBManager_hpp_ +#include "uORBDeviceNode.hpp" #include "uORBCommon.hpp" #include "uORBDeviceMaster.hpp" @@ -54,6 +55,7 @@ namespace uORB { class Manager; +class SubscriptionCallback; } /** @@ -165,7 +167,7 @@ public: * @param handle handle returned by orb_advertise or orb_advertise_multi. * @return 0 on success */ - int orb_unadvertise(orb_advert_t handle); + static int orb_unadvertise(orb_advert_t handle); /** * Publish new data to a topic. @@ -180,7 +182,7 @@ public: * @param data A pointer to the data to be published. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data); + static int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data); /** * Subscribe to a topic. @@ -301,7 +303,7 @@ public: * @param instance ORB instance * @return OK if the topic exists, PX4_ERROR otherwise. */ - int orb_exists(const struct orb_metadata *meta, int instance); + static int orb_exists(const struct orb_metadata *meta, int instance); /** * Set the minimum interval between which updates are seen for a subscription. @@ -335,6 +337,26 @@ public: */ int orb_get_interval(int handle, unsigned *interval); + static bool orb_device_node_exists(ORB_ID orb_id, uint8_t instance); + + static void *orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation); + + static void orb_remove_internal_subscriber(void *node_handle); + + static uint8_t orb_get_queue_size(const void *node_handle); + + static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation); + + static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub); + + static void unregister_callback(void *node_handle, SubscriptionCallback *callback_sub); + + static uint8_t orb_get_instance(const void *node_handle); + + static unsigned updates_available(const void *node_handle, unsigned last_generation) { return static_cast(node_handle)->updates_available(last_generation); } + + static bool is_advertised(const void *node_handle) { return static_cast(node_handle)->is_advertised(); } + #ifdef ORB_COMMUNICATOR /** * Method to set the uORBCommunicator::IChannel instance.