diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 543b64d89b..33af53276f 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -40,37 +40,23 @@ #pragma once template -class __EXPORT ListNode +class ListNode { public: - ListNode() : _sibling() {} - virtual ~ListNode() = default; - - // no copy, assignment, move, move assignment - ListNode(const ListNode &) = delete; - ListNode &operator=(const ListNode &) = delete; - ListNode(ListNode &&) = delete; - ListNode &operator=(ListNode &&) = delete; void setSibling(T sibling) { _sibling = sibling; } - const T getSibling() { return _sibling; } + const T getSibling() const { return _sibling; } protected: - T _sibling; + + T _sibling{nullptr}; + }; template -class __EXPORT List +class List { public: - List() : _head() {} - virtual ~List() = default; - - // no copy, assignment, move, move assignment - List(const List &) = delete; - List &operator=(const List &) = delete; - List(List &&) = delete; - List &operator=(List &&) = delete; void add(T newNode) { @@ -78,8 +64,9 @@ public: _head = newNode; } - const T getHead() { return _head; } + const T getHead() const { return _head; } protected: - T _head; + + T _head{nullptr}; }; diff --git a/src/lib/cdev/CDev.hpp b/src/lib/cdev/CDev.hpp index cecaa0f3cb..af036e8baa 100644 --- a/src/lib/cdev/CDev.hpp +++ b/src/lib/cdev/CDev.hpp @@ -155,6 +155,13 @@ public: */ virtual int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char *get_devname() const { return _devname; } + protected: /** * Pointer to the default cdev file operations table; useful for @@ -239,13 +246,6 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); - /** - * Get the device name. - * - * @return the file system string of the device handle - */ - const char *get_devname() { return _devname; } - /** * Take the driver lock. * diff --git a/src/modules/uORB/ORBMap.hpp b/src/modules/uORB/ORBMap.hpp deleted file mode 100644 index cb44936fda..0000000000 --- a/src/modules/uORB/ORBMap.hpp +++ /dev/null @@ -1,161 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -#pragma once - -#include -#include - - -namespace uORB -{ -class DeviceNode; -class ORBMap; -} - -class uORB::ORBMap -{ -public: - struct Node { - struct Node *next; - const char *node_name; - uORB::DeviceNode *node; - }; - - ORBMap() : - _top(nullptr), - _end(nullptr) - { } - ~ORBMap() - { - while (_top != nullptr) { - unlinkNext(_top); - - if (_top->next == nullptr) { - free(_top); - _top = nullptr; - _end = nullptr; - } - } - } - - /** - * Insert an element with a unique name - * @param node_name name of the node. This will not be copied, so the caller has to ensure - * the pointer is valid until the node is removed from ORBMap - * @param node - */ - void insert(const char *node_name, uORB::DeviceNode *node) - { - Node **p; - - if (_top == nullptr) { - p = &_top; - - } else { - p = &_end->next; - } - - *p = (Node *)malloc(sizeof(Node)); - - if (_end) { - _end = _end->next; - - } else { - _end = _top; - } - - _end->next = nullptr; - _end->node_name = node_name; - _end->node = node; - } - - bool find(const char *node_name) - { - Node *p = _top; - - while (p) { - if (strcmp(p->node_name, node_name) == 0) { - return true; - } - - p = p->next; - } - - return false; - } - - uORB::DeviceNode *get(const char *node_name) - { - Node *p = _top; - - while (p) { - if (strcmp(p->node_name, node_name) == 0) { - return p->node; - } - - p = p->next; - } - - return nullptr; - } - - Node *top() const - { - return _top; - } - - bool empty() const - { - return !_top; - } - -private: - void unlinkNext(Node *a) - { - Node *b = a->next; - - if (b != nullptr) { - if (_end == b) { - _end = a; - } - - a->next = b->next; - free(b); - } - } - - Node *_top; - Node *_end; -}; - diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index afccdc8e93..bd97e951e5 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -43,24 +43,6 @@ #include #include -#ifdef __PX4_NUTTX -#define ITERATE_NODE_MAP() \ - for (ORBMap::Node *node_iter = _node_map.top(); node_iter; node_iter = node_iter->next) -#define INIT_NODE_MAP_VARS(node_obj, node_name_str) \ - DeviceNode *node_obj = node_iter->node; \ - const char *node_name_str = node_iter->node_name; \ - UNUSED(node_name_str); - -#else -#include -#define ITERATE_NODE_MAP() \ - for (const auto &node_iter : _node_map) -#define INIT_NODE_MAP_VARS(node_obj, node_name_str) \ - DeviceNode *node_obj = node_iter.second; \ - const char *node_name_str = node_iter.first.c_str(); \ - UNUSED(node_name_str); -#endif - uORB::DeviceMaster::DeviceMaster() { px4_sem_init(&_lock, 0, 1); @@ -121,7 +103,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in } /* construct the new node */ - uORB::DeviceNode *node = new uORB::DeviceNode(meta, devpath, priority); + uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath, priority); /* if we didn't get a device, that's bad */ if (node == nullptr) { @@ -139,7 +121,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in 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); + uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries); if ((existing_node != nullptr) && !(existing_node->is_published())) { /* nothing has been published yet, lets claim it */ @@ -156,11 +138,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in } else { // add to the node map;. -#ifdef __PX4_NUTTX - _node_map.insert(devpath, node); -#else - _node_map[std::string(devpath)] = node; -#endif + _node_list.add(node); } group_tries++; @@ -184,9 +162,8 @@ void uORB::DeviceMaster::printStatistics(bool reset) bool had_print = false; lock(); - ITERATE_NODE_MAP() { - INIT_NODE_MAP_VARS(node, node_name) + for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) { if (node->print_statistics(reset)) { had_print = true; } @@ -212,8 +189,8 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node } } - ITERATE_NODE_MAP() { - INIT_NODE_MAP_VARS(node, node_name) + for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) { + ++num_topics; //check if already added @@ -255,8 +232,7 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node } last_node->node = node; - int node_name_len = strlen(node_name); - last_node->instance = (uint8_t)(node_name[node_name_len - 1] - '0'); + size_t name_length = strlen(last_node->node->get_meta()->o_name); if (name_length > max_topic_name_length) { @@ -286,7 +262,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) lock(); - if (_node_map.empty()) { + if (_node_list.getHead() == nullptr) { unlock(); PX4_INFO("no active topics"); return; @@ -374,7 +350,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) if (!print_active_only || cur_node->pub_msg_delta > 0) { PX4_INFO_RAW(CLEAR_LINE "%-*s %2i %4i %4i %5i %i\n", (int)max_topic_name_length, - cur_node->node->get_meta()->o_name, (int)cur_node->instance, + cur_node->node->get_meta()->o_name, (int)cur_node->node->get_instance(), (int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta, (int)cur_node->lost_msg_delta, cur_node->node->get_queue_size()); } @@ -403,39 +379,37 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) { lock(); - uORB::DeviceNode *node = getDeviceNodeLocked(nodepath); + + for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) { + if (strcmp(node->get_devname(), nodepath) == 0) { + unlock(); + return node; + } + } + unlock(); + + return nullptr; +} + +uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) +{ + lock(); + uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); + unlock(); + //We can safely return the node that can be used by any thread, because //a DeviceNode never gets deleted. return node; } - -#ifdef __PX4_NUTTX -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const char *nodepath) +uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance) { - uORB::DeviceNode *rc = nullptr; - - if (_node_map.find(nodepath)) { - rc = _node_map.get(nodepath); + for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) { + if ((strcmp(node->get_name(), meta->o_name) == 0) && (node->get_instance() == instance)) { + return node; + } } - return rc; + return nullptr; } - -#else - -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const char *nodepath) -{ - uORB::DeviceNode *rc = nullptr; - std::string np(nodepath); - - auto iter = _node_map.find(np); - - if (iter != _node_map.end()) { - rc = iter->second; - } - - return rc; -} -#endif diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index 202402a988..242201f275 100644 --- a/src/modules/uORB/uORBDeviceMaster.hpp +++ b/src/modules/uORB/uORBDeviceMaster.hpp @@ -45,17 +45,10 @@ class DeviceMaster; class Manager; } -#ifdef __PX4_NUTTX #include #include -#include "ORBMap.hpp" -#else - -#include -#include - -#endif /* __PX4_NUTTX */ +#include /** * Master control device for ObjDev. @@ -74,6 +67,7 @@ public: * @return node if exists, nullptr otherwise */ uORB::DeviceNode *getDeviceNode(const char *node_name); + uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance); /** * Print statistics for each existing topic. @@ -97,7 +91,6 @@ private: struct DeviceNodeStatisticsData { DeviceNode *node; - uint8_t instance; uint32_t last_lost_msg_count; unsigned int last_pub_msg_count; uint32_t lost_msg_delta; @@ -115,13 +108,10 @@ private: * _lock must already be held when calling this. * @return node if exists, nullptr otherwise */ - uORB::DeviceNode *getDeviceNodeLocked(const char *node_name); + uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance); + + List _node_list; -#ifdef __PX4_NUTTX - ORBMap _node_map; -#else - 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) */ diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index f7042b024a..e176598722 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -53,10 +53,12 @@ uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(cdev::file_t *fil return (SubscriberData *)(filp->f_priv); } -uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *path, int priority, unsigned int queue_size) : +uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, + uint8_t priority, uint8_t queue_size) : CDev(path), _meta(meta), - _priority((uint8_t)priority), + _instance(instance), + _priority(priority), _queue_size(queue_size) { } diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 34d86226b4..9e5c9916a1 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -38,6 +38,8 @@ #include +#include + namespace uORB { class DeviceNode; @@ -48,10 +50,11 @@ class Manager; /** * Per-object device instance. */ -class uORB::DeviceNode : public cdev::CDev +class uORB::DeviceNode : public cdev::CDev, public ListNode { public: - DeviceNode(const struct orb_metadata *meta, const char *path, int priority, unsigned int queue_size = 1); + DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t priority, + uint8_t queue_size = 1); ~DeviceNode(); // no copy, assignment, move, move assignment @@ -175,11 +178,19 @@ public: */ bool print_statistics(bool reset); - unsigned int get_queue_size() const { return _queue_size; } + uint8_t get_queue_size() const { return _queue_size; } + int8_t subscriber_count() const { return _subscriber_count; } + uint32_t lost_message_count() const { return _lost_messages; } - unsigned int published_message_count() const { return _generation; } - const struct orb_metadata *get_meta() const { return _meta; } + + unsigned published_message_count() const { return _generation; } + + const orb_metadata *get_meta() const { return _meta; } + + const char *get_name() const { return _meta->o_name; } + + uint8_t get_instance() const { return _instance; } int get_priority() const { return _priority; } void set_priority(uint8_t priority) { _priority = priority; } @@ -211,7 +222,8 @@ private: { if (update_interval) { update_interval->update_reported = update_reported_flag; } } }; - const struct orb_metadata *_meta; /**< object metadata information */ + const orb_metadata *_meta; /**< object metadata information */ + const uint8_t _instance; /**< orb multi instance identifier */ 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 */ @@ -220,20 +232,15 @@ private: uint8_t _queue_size; /**< maximum number of elements in the queue */ int8_t _subscriber_count{0}; + px4_task_t _publisher{0}; /**< if nonzero, current publisher. Only used inside the advertise call. + We allow one publisher to have an open file descriptor at the same time. */ + + // statistics + uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same + message, it is counted as two. */ + inline static SubscriberData *filp_to_sd(cdev::file_t *filp); -#ifdef __PX4_NUTTX - pid_t _publisher {0}; /**< if nonzero, current publisher. Only used inside the advertise call. - We allow one publisher to have an open file descriptor at the same time. */ -#else - px4_task_t _publisher {0}; /**< if nonzero, current publisher. Only used inside the advertise call. - We allow one publisher to have an open file descriptor at the same time. */ -#endif - - //statistics - uint32_t _lost_messages = 0; ///< nr of lost messages for all subscribers. If two subscribers lose the same - ///message, it is counted as two. - /** * Perform a deferred update for a rate-limited subscriber. */