From cfac2cc38e4b8f7687354ec672a402e81e0d46d7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 16 Sep 2018 22:43:32 -0400 Subject: [PATCH] uORB advertise through uORBDeviceMaster directly --- src/drivers/drv_orb_dev.h | 20 --- src/modules/uORB/uORBDeviceMaster.cpp | 194 +++++++++++++------------- src/modules/uORB/uORBDeviceMaster.hpp | 15 +- src/modules/uORB/uORBDeviceNode.cpp | 4 +- src/modules/uORB/uORBDeviceNode.hpp | 2 + src/modules/uORB/uORBManager.cpp | 55 ++------ src/modules/uORB/uORBManager.hpp | 2 +- 7 files changed, 118 insertions(+), 174 deletions(-) diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 7947e68ec4..060704deaa 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -45,29 +45,9 @@ #include #include -/* - * ioctl() definitions - */ - -/** path to the uORB control device for pub/sub topics */ -#define TOPIC_MASTER_DEVICE_PATH "/obj/_obj_" - -/** path to the uORB control device for parameter topics */ -#define PARAM_MASTER_DEVICE_PATH "/param/_param_" - -/** maximum ogbject name length */ -#define ORB_MAXNAME 32 - #define _ORBIOCBASE (0x2600) #define _ORBIOC(_n) (_PX4_IOC(_ORBIOCBASE, _n)) -/* - * IOCTLs for the uORB control device - */ - -/** Advertise a new topic described by *(uorb_metadata *)arg */ -#define ORBIOCADVERTISE _ORBIOC(0) - /* * IOCTLs for individual topics. */ diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index c3a821af4f..0684dee16e 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -61,122 +61,117 @@ UNUSED(node_name_str); #endif -uORB::DeviceMaster::DeviceMaster() : - CDev(TOPIC_MASTER_DEVICE_PATH) +uORB::DeviceMaster::DeviceMaster() { + px4_sem_init(&_lock, 0, 1); _last_statistics_output = hrt_absolute_time(); } -int -uORB::DeviceMaster::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) +uORB::DeviceMaster::~DeviceMaster() { - int ret; + px4_sem_destroy(&_lock); +} - switch (cmd) { - case ORBIOCADVERTISE: { - const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; - const struct orb_metadata *meta = adv->meta; - char nodepath[orb_maxpath]; +int +uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, int priority) +{ + int ret = PX4_ERROR; - /* construct a path to the node - this also checks the node name */ - ret = uORB::Utils::node_mkpath(nodepath, meta, adv->instance); + char nodepath[orb_maxpath]; - if (ret != PX4_OK) { - return ret; - } + /* construct a path to the node - this also checks the node name */ + ret = uORB::Utils::node_mkpath(nodepath, meta, instance); - ret = PX4_ERROR; + if (ret != PX4_OK) { + return ret; + } - /* try for topic groups */ - const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; + ret = PX4_ERROR; - if (adv->instance) { - /* for an advertiser, this will be 0, but a for subscriber that requests a certain instance, - * we do not want to start with 0, but with the instance the subscriber actually requests. - */ - group_tries = *adv->instance; + /* try for topic groups */ + const unsigned max_group_tries = (instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; - if (group_tries >= max_group_tries) { - return -ENOMEM; - } - } + if (instance) { + /* for an advertiser, this will be 0, but a for subscriber that requests a certain instance, + * we do not want to start with 0, but with the instance the subscriber actually requests. + */ + group_tries = *instance; - SmartLock smart_lock(_lock); + if (group_tries >= max_group_tries) { + return -ENOMEM; + } + } - do { - /* if path is modifyable change try index */ - if (adv->instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *(adv->instance) = group_tries; - } + SmartLock smart_lock(_lock); - /* driver wants a permanent copy of the path, so make one here */ - const char *devpath = strdup(nodepath); - - if (devpath == nullptr) { - return -ENOMEM; - } - - /* construct the new node */ - uORB::DeviceNode *node = new uORB::DeviceNode(meta, devpath, adv->priority); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - free((void *)devpath); - return -ENOMEM; - } - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); - - /* if init failed, discard the node and its name */ - if (ret != PX4_OK) { - delete node; - - if (ret == -EEXIST) { - /* if the node exists already, get the existing one and check if - * something has been published yet. */ - uORB::DeviceNode *existing_node = getDeviceNodeLocked(devpath); - - if ((existing_node != nullptr) && !(existing_node->is_published())) { - /* nothing has been published yet, lets claim it */ - existing_node->set_priority(adv->priority); - ret = PX4_OK; - - } else { - /* otherwise: data has already been published, keep looking */ - } - } - - /* also discard the name now */ - free((void *)devpath); - - } else { - // add to the node map;. -#ifdef __PX4_NUTTX - _node_map.insert(devpath, node); -#else - _node_map[std::string(devpath)] = node; -#endif - } - - group_tries++; - - } while (ret != PX4_OK && (group_tries < max_group_tries)); - - if (ret != PX4_OK && group_tries >= max_group_tries) { - ret = -ENOMEM; - } - - return ret; + do { + /* if path is modifyable change try index */ + if (instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *instance = group_tries; } - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); + /* driver wants a permanent copy of the path, so make one here */ + const char *devpath = strdup(nodepath); + + if (devpath == nullptr) { + return -ENOMEM; + } + + /* construct the new node */ + uORB::DeviceNode *node = new uORB::DeviceNode(meta, devpath, priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + free((void *)devpath); + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != PX4_OK) { + delete node; + + if (ret == -EEXIST) { + /* if the node exists already, get the existing one and check if + * something has been published yet. */ + uORB::DeviceNode *existing_node = getDeviceNodeLocked(devpath); + + if ((existing_node != nullptr) && !(existing_node->is_published())) { + /* nothing has been published yet, lets claim it */ + existing_node->set_priority(priority); + ret = PX4_OK; + + } else { + /* otherwise: data has already been published, keep looking */ + } + } + + /* also discard the name now */ + free((void *)devpath); + + } else { + // add to the node map;. +#ifdef __PX4_NUTTX + _node_map.insert(devpath, node); +#else + _node_map[std::string(devpath)] = node; +#endif + } + + group_tries++; + + } while (ret != PX4_OK && (group_tries < max_group_tries)); + + if (ret != PX4_OK && group_tries >= max_group_tries) { + ret = -ENOMEM; } + + return ret; } void uORB::DeviceMaster::printStatistics(bool reset) @@ -207,7 +202,7 @@ void uORB::DeviceMaster::printStatistics(bool reset) void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, size_t &max_topic_name_length, char **topic_filter, int num_filters) { - DeviceNodeStatisticsData *cur_node; + DeviceNodeStatisticsData *cur_node = nullptr; num_topics = 0; DeviceNodeStatisticsData *last_node = *first_node; @@ -277,7 +272,6 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) { - bool print_active_only = true; if (topic_filter && num_filters > 0) { diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index 0410a68ca7..202402a988 100644 --- a/src/modules/uORB/uORBDeviceMaster.hpp +++ b/src/modules/uORB/uORBDeviceMaster.hpp @@ -34,8 +34,9 @@ #pragma once #include + #include "uORBCommon.hpp" -#include +#include namespace uORB { @@ -62,10 +63,11 @@ class Manager; * Used primarily to create new objects via the ORBIOCCREATE * ioctl. */ -class uORB::DeviceMaster : public cdev::CDev +class uORB::DeviceMaster { public: - virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg); + + int advertise(const struct orb_metadata *meta, int *instance, int priority); /** * Public interface for getDeviceNodeLocked(). Takes care of synchronization. @@ -91,7 +93,7 @@ public: private: // Private constructor, uORB::Manager takes care of its creation DeviceMaster(); - virtual ~DeviceMaster() = default; + ~DeviceMaster(); struct DeviceNodeStatisticsData { DeviceNode *node; @@ -121,4 +123,9 @@ private: std::map _node_map; #endif hrt_abstime _last_statistics_output; + + px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */ + + void lock() { do {} while (px4_sem_wait(&_lock) != 0); } + void unlock() { px4_sem_post(&_lock); } }; diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index 3057d65f33..add2722d9c 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -397,8 +397,8 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; int ret; - /* check if the device handle is initialized */ - if ((devnode == nullptr) || (meta == nullptr)) { + /* check if the device handle is initialized and data is valid */ + if ((devnode == nullptr) || (meta == nullptr) || (data == nullptr)) { errno = EFAULT; return PX4_ERROR; } diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 9a84943aa4..c480729779 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -36,6 +36,8 @@ #include "uORBCommon.hpp" #include "uORBDeviceMaster.hpp" +#include + namespace uORB { class DeviceNode; diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index fbba90ea45..027626c45b 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -44,12 +44,8 @@ #include "uORBUtils.hpp" #include "uORBManager.hpp" - -//========================= Static initializations ================= uORB::Manager *uORB::Manager::_Instance = nullptr; -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- bool uORB::Manager::initialize() { if (_Instance == nullptr) { @@ -59,8 +55,6 @@ bool uORB::Manager::initialize() return _Instance != nullptr; } -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- uORB::Manager::Manager() { #ifdef ORB_USE_PUBLISHER_RULES @@ -89,17 +83,7 @@ uORB::DeviceMaster *uORB::Manager::get_device_master() if (!_device_master) { _device_master = new DeviceMaster(); - if (_device_master) { - int ret = _device_master->init(); - - if (ret != PX4_OK) { - PX4_ERR("Initialization of DeviceMaster failed (%i)", ret); - errno = -ret; - delete _device_master; - _device_master = nullptr; - } - - } else { + if (_device_master == nullptr) { PX4_ERR("Failed to allocate DeviceMaster"); errno = ENOMEM; } @@ -188,7 +172,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, #endif /* ORB_USE_PUBLISHER_RULES */ /* open the node as an advertiser */ - int fd = node_open(meta, data, true, instance, priority); + int fd = node_open(meta, true, instance, priority); if (fd == PX4_ERROR) { PX4_ERR("%s advertise failed", meta->o_name); @@ -246,13 +230,13 @@ int uORB::Manager::orb_unadvertise(orb_advert_t handle) int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) { - return node_open(meta, nullptr, false); + 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, nullptr, false, &inst); + return node_open(meta, false, &inst); } int uORB::Manager::orb_unsubscribe(int fd) @@ -322,42 +306,27 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval) int uORB::Manager::node_advertise(const struct orb_metadata *meta, int *instance, int priority) { - int fd = -1; int ret = PX4_ERROR; /* fill advertiser data */ - const struct orb_advertdata adv = { meta, instance, priority }; - /* open the control device */ - fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); - - if (fd < 0) { - goto out; + if (get_device_master()) { + ret = _device_master->advertise(meta, instance, priority); } - /* advertise the object */ - ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); - /* it's PX4_OK if it already exists */ if ((PX4_OK != ret) && (EEXIST == errno)) { ret = PX4_OK; } -out: - - if (fd >= 0) { - px4_close(fd); - } - return ret; } -int uORB::Manager::node_open(const struct orb_metadata *meta, const void *data, bool advertiser, int *instance, - int priority) +int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance, int priority) { char path[orb_maxpath]; int fd = -1; - int ret = -1; + int ret = PX4_ERROR; /* * If meta is null, the object was not defined, i.e. it is not @@ -368,14 +337,6 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, const void *data, return PX4_ERROR; } - /* - * Advertiser must publish an initial value. - */ - if (advertiser && (data == nullptr)) { - errno = EINVAL; - 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) { diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index a9ff60a58a..42d195b26f 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -398,7 +398,7 @@ private: // class methods * Handles creation of the object and the initial publication for * advertisers. */ - int node_open(const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, + int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT); private: // data members