fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 13:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

uORB replace ORBMap with linked list

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uORB/uORBDeviceMaster.cpp
+33 -59
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