diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index 5838d8c40e..0f0060836b 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -34,7 +34,9 @@ # this includes the generated topics directory include_directories(${CMAKE_CURRENT_BINARY_DIR}) -px4_add_library(uORB +set(SRCS) + +set(SRCS_COMMON ORBSet.hpp Publication.hpp PublicationMulti.hpp @@ -47,18 +49,49 @@ px4_add_library(uORB uORB.h uORBCommon.hpp uORBCommunicator.hpp - uORBDeviceMaster.cpp - uORBDeviceMaster.hpp - uORBDeviceNode.cpp - uORBDeviceNode.hpp - uORBManager.cpp uORBManager.hpp uORBUtils.cpp uORBUtils.hpp -) + uORBDeviceMaster.hpp + uORBDeviceNode.hpp + ) + +set(SRCS_KERNEL + uORBDeviceMaster.cpp + uORBDeviceNode.cpp + uORBManager.cpp + ) + +set(SRCS_USER + uORBManagerUsr.cpp + ) + +if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx") + # Kernel side library in nuttx kernel/protected build + px4_add_library(uORB_kernel + ${SRCS_COMMON} + ${SRCS_KERNEL} + ) + target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs) + target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__) + + # User side library in nuttx kernel/protected build + px4_add_library(uORB + ${SRCS_COMMON} + ${SRCS_USER} + ) +else() + + # Library for all other targets (flat build, posix...) + px4_add_library(uORB + ${SRCS_COMMON} + ${SRCS_KERNEL} + ) + target_link_libraries(uORB PRIVATE cdev) +endif() +target_link_libraries(uORB PRIVATE uorb_msgs) target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_link_libraries(uORB PRIVATE cdev uorb_msgs) if(PX4_TESTING) add_subdirectory(uORB_tests) diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 786106235f..61b59d84ee 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -41,10 +41,15 @@ #include "uORBManager.hpp" #include "uORBCommon.hpp" + #include #include #include +#ifdef __PX4_NUTTX +#include +#endif + static uORB::DeviceMaster *g_dev = nullptr; int uorb_start(void) @@ -60,6 +65,7 @@ int uorb_start(void) return -ENOMEM; } +#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) /* create the driver */ g_dev = uORB::Manager::get_instance()->get_device_master(); @@ -67,11 +73,15 @@ int uorb_start(void) return -errno; } +#endif + return OK; } int uorb_status(void) { +#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) + if (g_dev != nullptr) { g_dev->printStatistics(); @@ -79,11 +89,16 @@ int uorb_status(void) PX4_INFO("uorb is not running"); } +#else + boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_STATUS); +#endif return OK; } int uorb_top(char **topic_filter, int num_filters) { +#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) + if (g_dev != nullptr) { g_dev->showTop(topic_filter, num_filters); @@ -91,6 +106,9 @@ int uorb_top(char **topic_filter, int num_filters) PX4_INFO("uorb is not running"); } +#else + boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_TOP); +#endif return OK; } diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 97f156d125..281668dba6 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -40,6 +40,10 @@ #include #include +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) +#include +#endif + #include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" @@ -52,6 +56,9 @@ bool uORB::Manager::initialize() _Instance = new uORB::Manager(); } +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) + px4_register_boardct_ioctl(_ORBIOCBASE, orb_ioctl); +#endif return _Instance != nullptr; } @@ -103,6 +110,124 @@ uORB::DeviceMaster *uORB::Manager::get_device_master() return _device_master; } +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) +int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) +{ + int ret = PX4_OK; + + switch (cmd) { + case ORBIOCDEVEXISTS: { + orbiocdevexists_t *data = (orbiocdevexists_t *)arg; + + if (data->check_advertised) { + data->ret = uORB::Manager::orb_exists(get_orb_meta(data->orb_id), data->instance); + + } else { + data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR; + } + } + break; + + case ORBIOCDEVADVERTISE: { + orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg; + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + + if (dev) { + data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance); + + } else { + data->ret = PX4_ERROR; + } + } + break; + + case ORBIOCDEVUNADVERTISE: { + orbiocdevunadvertise_t *data = (orbiocdevunadvertise_t *)arg; + data->ret = uORB::Manager::orb_unadvertise(data->handle); + } + break; + + case ORBIOCDEVPUBLISH: { + orbiocdevpublish_t *data = (orbiocdevpublish_t *)arg; + data->ret = uORB::Manager::orb_publish(data->meta, data->handle, data->data); + } + break; + + case ORBIOCDEVADDSUBSCRIBER: { + orbiocdevaddsubscriber_t *data = (orbiocdevaddsubscriber_t *)arg; + data->handle = uORB::Manager::orb_add_internal_subscriber(data->orb_id, data->instance, data->initial_generation); + } + break; + + case ORBIOCDEVREMSUBSCRIBER: { + uORB::Manager::orb_remove_internal_subscriber(reinterpret_cast(arg)); + } + break; + + case ORBIOCDEVQUEUESIZE: { + orbiocdevqueuesize_t *data = (orbiocdevqueuesize_t *)arg; + data->size = uORB::Manager::orb_get_queue_size(data->handle); + } + break; + + case ORBIOCDEVDATACOPY: { + orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg; + data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation); + } + break; + + case ORBIOCDEVREGCALLBACK: { + orbiocdevregcallback_t *data = (orbiocdevregcallback_t *)arg; + data->registered = uORB::Manager::register_callback(data->handle, data->callback_sub); + } + break; + + case ORBIOCDEVUNREGCALLBACK: { + orbiocdevunregcallback_t *data = (orbiocdevunregcallback_t *)arg; + uORB::Manager::unregister_callback(data->handle, data->callback_sub); + } + break; + + case ORBIOCDEVGETINSTANCE: { + orbiocdevgetinstance_t *data = (orbiocdevgetinstance_t *)arg; + data->instance = uORB::Manager::orb_get_instance(data->handle); + } + break; + + case ORBIOCDEVMASTERCMD: { + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + + if (dev) { + if (arg == ORB_DEVMASTER_TOP) { + dev->showTop(nullptr, 0); + + } else { + dev->printStatistics(); + } + } + } + break; + + case ORBIOCDEVUPDATESAVAIL: { + orbiocdevupdatesavail_t *data = (orbiocdevupdatesavail_t *)arg; + data->ret = updates_available(data->handle, data->last_generation); + } + break; + + case ORBIOCDEVISADVERTISED: { + orbiocdevisadvertised_t *data = (orbiocdevisadvertised_t *)arg; + data->ret = is_advertised(data->handle); + } + break; + + default: + ret = -ENOTTY; + } + + return ret; +} +#endif + int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { int ret = PX4_ERROR; @@ -380,6 +505,19 @@ uint8_t uORB::Manager::orb_get_instance(const void *node_handle) return -1; } +/* These are optimized by inlining in NuttX Flat build */ +#if !defined(CONFIG_BUILD_FLAT) +unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) +{ + return static_cast(node_handle)->updates_available(last_generation); +} + +bool uORB::Manager::is_advertised(const void *node_handle) +{ + return static_cast(node_handle)->is_advertised(); +} +#endif + 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 647f10a3ed..728c83f44b 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -38,6 +38,7 @@ #include "uORBCommon.hpp" #include "uORBDeviceMaster.hpp" +#include // For ORB_ID enum #include #ifdef __PX4_NUTTX @@ -58,6 +59,105 @@ class Manager; class SubscriptionCallback; } + +/* + * IOCTLs for manager to access device nodes using + * a handle + */ + +#define ORBIOCDEVEXISTS _ORBIOC(30) +typedef struct orbiocdevexists { + const ORB_ID orb_id; + const uint8_t instance; + const bool check_advertised; + int ret; +} orbiocdevexists_t; + +#define ORBIOCDEVADVERTISE _ORBIOC(31) +typedef struct orbiocadvertise { + const struct orb_metadata *meta; + bool is_advertiser; + int *instance; + int ret; +} orbiocdevadvertise_t; + +#define ORBIOCDEVUNADVERTISE _ORBIOC(32) +typedef struct orbiocunadvertise { + void *handle; + int ret; +} orbiocdevunadvertise_t; + +#define ORBIOCDEVPUBLISH _ORBIOC(33) +typedef struct orbiocpublish { + const struct orb_metadata *meta; + orb_advert_t handle; + const void *data; + int ret; +} orbiocdevpublish_t; + +#define ORBIOCDEVADDSUBSCRIBER _ORBIOC(34) +typedef struct { + const ORB_ID orb_id; + const uint8_t instance; + unsigned *initial_generation; + void *handle; +} orbiocdevaddsubscriber_t; + +#define ORBIOCDEVREMSUBSCRIBER _ORBIOC(35) + +#define ORBIOCDEVQUEUESIZE _ORBIOC(36) +typedef struct { + const void *handle; + uint8_t size; +} orbiocdevqueuesize_t; + +#define ORBIOCDEVDATACOPY _ORBIOC(37) +typedef struct { + void *handle; + void *dst; + unsigned generation; + bool ret; +} orbiocdevdatacopy_t; + +#define ORBIOCDEVREGCALLBACK _ORBIOC(38) +typedef struct { + void *handle; + class uORB::SubscriptionCallback *callback_sub; + bool registered; +} orbiocdevregcallback_t; + +#define ORBIOCDEVUNREGCALLBACK _ORBIOC(39) +typedef struct { + void *handle; + class uORB::SubscriptionCallback *callback_sub; +} orbiocdevunregcallback_t; + +#define ORBIOCDEVGETINSTANCE _ORBIOC(40) +typedef struct { + const void *handle; + uint8_t instance; +} orbiocdevgetinstance_t; + +#define ORBIOCDEVUPDATESAVAIL _ORBIOC(41) +typedef struct { + const void *handle; + unsigned last_generation; + unsigned ret; +} orbiocdevupdatesavail_t; + +#define ORBIOCDEVISADVERTISED _ORBIOC(42) +typedef struct { + const void *handle; + bool ret; +} orbiocdevisadvertised_t; + +typedef enum { + ORB_DEVMASTER_STATUS = 0, + ORB_DEVMASTER_TOP = 1 +} orbiocdevmastercmd_t; +#define ORBIOCDEVMASTERCMD _ORBIOC(45) + + /** * This is implemented as a singleton. This class manages creating the * uORB nodes for each uORB topics and also implements the behavor of the @@ -98,6 +198,10 @@ public: */ uORB::DeviceMaster *get_device_master(); +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) + static int orb_ioctl(unsigned int cmd, unsigned long arg); +#endif + // ==== uORB interface methods ==== /** * Advertise as the publisher of a topic. @@ -353,9 +457,16 @@ public: static uint8_t orb_get_instance(const void *node_handle); +#if defined(CONFIG_BUILD_FLAT) + /* These are optimized by inlining in NuttX Flat build */ 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(); } +#else + static unsigned updates_available(const void *node_handle, unsigned last_generation); + + static bool is_advertised(const void *node_handle); +#endif #ifdef ORB_COMMUNICATOR /** diff --git a/platforms/common/uORB/uORBManagerUsr.cpp b/platforms/common/uORB/uORBManagerUsr.cpp new file mode 100644 index 0000000000..368e98119f --- /dev/null +++ b/platforms/common/uORB/uORBManagerUsr.cpp @@ -0,0 +1,349 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "uORBDeviceNode.hpp" +#include "uORBUtils.hpp" +#include "uORBManager.hpp" + +uORB::Manager *uORB::Manager::_Instance = nullptr; + +bool uORB::Manager::initialize() +{ + if (_Instance == nullptr) { + _Instance = new uORB::Manager(); + } + + return _Instance != nullptr; +} + +bool uORB::Manager::terminate() +{ + if (_Instance != nullptr) { + delete _Instance; + _Instance = nullptr; + return true; + } + + return false; +} + +uORB::Manager::Manager() +{ +} + +uORB::Manager::~Manager() +{ +} + +int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) +{ + // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) + if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) { + return PX4_ERROR; + } + + orbiocdevexists_t data = {static_cast(meta->o_id), static_cast(instance), true, PX4_ERROR}; + boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); + + return data.ret; +} + +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + unsigned int queue_size) +{ + /* open the node as an advertiser */ + int fd = node_open(meta, true, instance); + + if (fd == PX4_ERROR) { + PX4_ERR("%s advertise failed (%i)", meta->o_name, errno); + return nullptr; + } + + /* Set the queue size. This must be done before the first publication; thus it fails if + * this is not the first advertiser. + */ + int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); + + if (result < 0 && queue_size > 1) { + PX4_WARN("orb_advertise_multi: failed to set queue size"); + } + + /* get the advertiser handle and close the node */ + orb_advert_t advertiser; + + result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + px4_close(fd); + + if (result == PX4_ERROR) { + PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); + return nullptr; + } + + /* the advertiser may perform an initial publish to initialise the object */ + if (data != nullptr) { + result = orb_publish(meta, advertiser, data); + + if (result == PX4_ERROR) { + PX4_ERR("orb_publish failed %s", meta->o_name); + return nullptr; + } + } + + return advertiser; +} + +int uORB::Manager::orb_unadvertise(orb_advert_t handle) +{ + orbiocdevunadvertise_t data = {handle, PX4_ERROR}; + boardctl(ORBIOCDEVUNADVERTISE, reinterpret_cast(&data)); + + return data.ret; +} + +int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) +{ + return node_open(meta, false); +} + +int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(meta, false, &inst); +} + +int uORB::Manager::orb_unsubscribe(int fd) +{ + return px4_close(fd); +} + +int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ + orbiocdevpublish_t d = {meta, handle, data, PX4_ERROR}; + boardctl(ORBIOCDEVPUBLISH, reinterpret_cast(&d)); + + return d.ret; +} + +int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +{ + int ret; + + ret = px4_read(handle, buffer, meta->o_size); + + if (ret < 0) { + return PX4_ERROR; + } + + if (ret != (int)meta->o_size) { + errno = EIO; + return PX4_ERROR; + } + + return PX4_OK; +} + +int uORB::Manager::orb_check(int handle, bool *updated) +{ + /* Set to false here so that if `px4_ioctl` fails to false. */ + *updated = false; + return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); +} + +int uORB::Manager::orb_set_interval(int handle, unsigned interval) +{ + return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); +} + +int uORB::Manager::orb_get_interval(int handle, unsigned *interval) +{ + int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); + *interval /= 1000; + return ret; +} + +int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) +{ + char path[orb_maxpath]; + int fd = -1; + int ret = PX4_ERROR; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return PX4_ERROR; + } + + /* if we have an instance and are an advertiser, we will generate a new node and set the instance, + * so we do not need to open here */ + if (!instance || !advertiser) { + /* + * Generate the path to the node and try to open it. + */ + ret = uORB::Utils::node_mkpath(path, meta, instance); + + if (ret != OK) { + errno = -ret; + return PX4_ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY); + + } else { + *instance = 0; + } + + /* we may need to advertise the node... */ + if (fd < 0) { + ret = PX4_ERROR; + + orbiocdevadvertise_t data = {meta, advertiser, instance, PX4_ERROR}; + boardctl(ORBIOCDEVADVERTISE, (unsigned long)&data); + ret = data.ret; + + /* it's OK if it already exists */ + if ((ret != PX4_OK) && (EEXIST == errno)) { + ret = PX4_OK; + } + + if (ret == PX4_OK) { + /* update the path, as it might have been updated during the node advertise call */ + ret = uORB::Utils::node_mkpath(path, meta, instance); + + /* on success, try to open again */ + if (ret == PX4_OK) { + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + + } else { + errno = -ret; + return PX4_ERROR; + } + } + } + + if (fd < 0) { + errno = EIO; + return PX4_ERROR; + } + + /* everything has been OK, we can return the handle now */ + return fd; +} + +bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) +{ + orbiocdevexists_t data = {orb_id, instance, false, 0}; + boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); + + return data.ret == PX4_OK ? true : false; +} + +void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) +{ + orbiocdevaddsubscriber_t data = {orb_id, instance, initial_generation, nullptr}; + boardctl(ORBIOCDEVADDSUBSCRIBER, reinterpret_cast(&data)); + + return data.handle; +} + +void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) +{ + boardctl(ORBIOCDEVREMSUBSCRIBER, reinterpret_cast(node_handle)); +} + +uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) +{ + orbiocdevqueuesize_t data = {node_handle, 0}; + boardctl(ORBIOCDEVQUEUESIZE, reinterpret_cast(&data)); + + return data.size; +} + +bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation) +{ + orbiocdevdatacopy_t data = {node_handle, dst, generation, false}; + boardctl(ORBIOCDEVDATACOPY, reinterpret_cast(&data)); + generation = data.generation; + + return data.ret; +} + +bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + orbiocdevregcallback_t data = {node_handle, callback_sub, false}; + boardctl(ORBIOCDEVREGCALLBACK, reinterpret_cast(&data)); + + return data.registered; +} + +void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + orbiocdevunregcallback_t data = {node_handle, callback_sub}; + boardctl(ORBIOCDEVUNREGCALLBACK, reinterpret_cast(&data)); +} + +uint8_t uORB::Manager::orb_get_instance(const void *node_handle) +{ + orbiocdevgetinstance_t data = {node_handle, 0}; + boardctl(ORBIOCDEVGETINSTANCE, reinterpret_cast(&data)); + + return data.instance; +} + +unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) +{ + orbiocdevupdatesavail_t data = {node_handle, last_generation, 0}; + boardctl(ORBIOCDEVUPDATESAVAIL, reinterpret_cast(&data)); + return data.ret; +} + +bool uORB::Manager::is_advertised(const void *node_handle) +{ + orbiocdevisadvertised_t data = {node_handle, false}; + boardctl(ORBIOCDEVISADVERTISED, reinterpret_cast(&data)); + return data.ret; +}