dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

uORB replace ORBMap with linked list

This commit is contained in:
Daniel Agar 2018-11-20 23:10:48 -05:00 committed by Beat Küng
parent 180cd94978
commit 023e267e9b
7 changed files with 83 additions and 284 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/include/containers/List.hpp
View File

@ -40,37 +40,23 @@
#pragma once
template<class T>
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 T>
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};
};

14
src/lib/cdev/CDev.hpp
View File

@ -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.
*

161
src/modules/uORB/ORBMap.hpp
View File

@ -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 <string.h>
#include <stdlib.h>
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;
};

92
src/modules/uORB/uORBDeviceMaster.cpp
View File

@ -43,24 +43,6 @@
#include <px4_sem.hpp>
#include <systemlib/px4_macros.h>
#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 <algorithm>
#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

20
src/modules/uORB/uORBDeviceMaster.hpp
View File

@ -45,17 +45,10 @@ class DeviceMaster;
class Manager;
}
#ifdef __PX4_NUTTX
#include <string.h>
#include <stdlib.h>
#include "ORBMap.hpp"
#else
#include <string>
#include <map>
#endif /* __PX4_NUTTX */
#include <containers/List.hpp>
/**
* 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<uORB::DeviceNode *> _node_list;
#ifdef __PX4_NUTTX
ORBMap _node_map;
#else
std::map<std::string, uORB::DeviceNode *> _node_map;
#endif
hrt_abstime _last_statistics_output;
px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */

6
src/modules/uORB/uORBDeviceNode.cpp
View File

@ -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)
{
}

43
src/modules/uORB/uORBDeviceNode.hpp
View File

@ -38,6 +38,8 @@
#include <lib/cdev/CDev.hpp>
#include <containers/List.hpp>
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<uORB::DeviceNode *>
{
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.
*/