mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 01:12:11 +00:00
uORB: split uORBDevices into uORBDeviceNode and uORBDeviceMaster
This commit is contained in:
Daniel Agar
GitHub
@@ -42,10 +42,11 @@ px4_add_module(
|
||||
Publication.cpp
|
||||
Subscription.cpp
|
||||
uORB.cpp
|
||||
uORBDevices.cpp
|
||||
uORBDeviceMaster.cpp
|
||||
uORBDeviceNode.cpp
|
||||
uORBMain.cpp
|
||||
uORBManager.cpp
|
||||
uORBUtils.cpp
|
||||
DEPENDS
|
||||
uorb_msgs
|
||||
)
|
||||
)
|
||||
|
||||
459
src/modules/uORB/uORBDeviceMaster.cpp
Normal file
459
src/modules/uORB/uORBDeviceMaster.cpp
Normal file
@@ -0,0 +1,459 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2016 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 "uORBDeviceMaster.hpp"
|
||||
#include "uORBDeviceNode.hpp"
|
||||
#include "uORBManager.hpp"
|
||||
#include "uORBUtils.hpp"
|
||||
|
||||
#ifdef ORB_COMMUNICATOR
|
||||
#include "uORBCommunicator.hpp"
|
||||
#endif /* ORB_COMMUNICATOR */
|
||||
|
||||
#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
|
||||
|
||||
using namespace device;
|
||||
|
||||
uORB::DeviceMaster::DeviceMaster() :
|
||||
CDev("obj_master", TOPIC_MASTER_DEVICE_PATH)
|
||||
{
|
||||
_last_statistics_output = hrt_absolute_time();
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret;
|
||||
|
||||
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];
|
||||
|
||||
/* construct a path to the node - this also checks the node name */
|
||||
ret = uORB::Utils::node_mkpath(nodepath, meta, adv->instance);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = PX4_ERROR;
|
||||
|
||||
/* try for topic groups */
|
||||
const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
|
||||
unsigned group_tries = 0;
|
||||
|
||||
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;
|
||||
|
||||
if (group_tries >= max_group_tries) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
}
|
||||
|
||||
SmartLock smart_lock(_lock);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
const char *objname = meta->o_name; //no need for a copy, meta->o_name will never be freed or changed
|
||||
|
||||
/* 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, objname, 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;
|
||||
}
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
void uORB::DeviceMaster::printStatistics(bool reset)
|
||||
{
|
||||
hrt_abstime current_time = hrt_absolute_time();
|
||||
PX4_INFO("Statistics, since last output (%i ms):", (int)((current_time - _last_statistics_output) / 1000));
|
||||
_last_statistics_output = current_time;
|
||||
|
||||
PX4_INFO("TOPIC, NR LOST MSGS");
|
||||
bool had_print = false;
|
||||
|
||||
lock();
|
||||
ITERATE_NODE_MAP() {
|
||||
INIT_NODE_MAP_VARS(node, node_name)
|
||||
|
||||
if (node->print_statistics(reset)) {
|
||||
had_print = true;
|
||||
}
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
if (!had_print) {
|
||||
PX4_INFO("No lost messages");
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
num_topics = 0;
|
||||
DeviceNodeStatisticsData *last_node = *first_node;
|
||||
|
||||
if (last_node) {
|
||||
while (last_node->next) {
|
||||
last_node = last_node->next;
|
||||
}
|
||||
}
|
||||
|
||||
ITERATE_NODE_MAP() {
|
||||
INIT_NODE_MAP_VARS(node, node_name)
|
||||
++num_topics;
|
||||
|
||||
//check if already added
|
||||
cur_node = *first_node;
|
||||
|
||||
while (cur_node && cur_node->node != node) {
|
||||
cur_node = cur_node->next;
|
||||
}
|
||||
|
||||
if (cur_node) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (num_filters > 0 && topic_filter) {
|
||||
bool matched = false;
|
||||
|
||||
for (int i = 0; i < num_filters; ++i) {
|
||||
if (strstr(node->get_meta()->o_name, topic_filter[i])) {
|
||||
matched = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!matched) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (last_node) {
|
||||
last_node->next = new DeviceNodeStatisticsData();
|
||||
last_node = last_node->next;
|
||||
|
||||
} else {
|
||||
*first_node = last_node = new DeviceNodeStatisticsData();
|
||||
}
|
||||
|
||||
if (!last_node) {
|
||||
PX4_ERR("mem alloc failed");
|
||||
break;
|
||||
}
|
||||
|
||||
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) {
|
||||
max_topic_name_length = name_length;
|
||||
}
|
||||
|
||||
last_node->last_lost_msg_count = last_node->node->lost_message_count();
|
||||
last_node->last_pub_msg_count = last_node->node->published_message_count();
|
||||
}
|
||||
}
|
||||
|
||||
#define CLEAR_LINE "\033[K"
|
||||
|
||||
void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
|
||||
{
|
||||
|
||||
bool print_active_only = true;
|
||||
|
||||
if (topic_filter && num_filters > 0) {
|
||||
if (!strcmp("-a", topic_filter[0])) {
|
||||
num_filters = 0;
|
||||
}
|
||||
|
||||
print_active_only = false; // print non-active if -a or some filter given
|
||||
}
|
||||
|
||||
PX4_INFO_RAW("\033[2J\n"); //clear screen
|
||||
|
||||
lock();
|
||||
|
||||
if (_node_map.empty()) {
|
||||
unlock();
|
||||
PX4_INFO("no active topics");
|
||||
return;
|
||||
}
|
||||
|
||||
DeviceNodeStatisticsData *first_node = nullptr;
|
||||
DeviceNodeStatisticsData *cur_node = nullptr;
|
||||
size_t max_topic_name_length = 0;
|
||||
int num_topics = 0;
|
||||
addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters);
|
||||
|
||||
/* a DeviceNode is never deleted, so it's save to unlock here and still access the DeviceNodes */
|
||||
unlock();
|
||||
|
||||
#ifdef __PX4_QURT //QuRT has no poll()
|
||||
int num_runs = 0;
|
||||
#else
|
||||
const int stdin_fileno = 0;
|
||||
|
||||
struct pollfd fds;
|
||||
fds.fd = stdin_fileno;
|
||||
fds.events = POLLIN;
|
||||
#endif
|
||||
bool quit = false;
|
||||
|
||||
hrt_abstime start_time = hrt_absolute_time();
|
||||
|
||||
while (!quit) {
|
||||
|
||||
#ifdef __PX4_QURT
|
||||
|
||||
if (++num_runs > 1) {
|
||||
quit = true; //just exit after one output
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
/* Sleep 200 ms waiting for user input five times ~ 1s */
|
||||
for (int k = 0; k < 5; k++) {
|
||||
char c;
|
||||
|
||||
int ret = ::poll(&fds, 1, 0); //just want to check if there is new data available
|
||||
|
||||
if (ret > 0) {
|
||||
|
||||
ret = ::read(stdin_fileno, &c, 1);
|
||||
|
||||
if (ret) {
|
||||
quit = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
usleep(200000);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
if (!quit) {
|
||||
|
||||
//update the stats
|
||||
hrt_abstime current_time = hrt_absolute_time();
|
||||
float dt = (current_time - start_time) / 1.e6f;
|
||||
cur_node = first_node;
|
||||
|
||||
while (cur_node) {
|
||||
uint32_t num_lost = cur_node->node->lost_message_count();
|
||||
unsigned int num_msgs = cur_node->node->published_message_count();
|
||||
cur_node->pub_msg_delta = (num_msgs - cur_node->last_pub_msg_count) / dt;
|
||||
cur_node->lost_msg_delta = (num_lost - cur_node->last_lost_msg_count) / dt;
|
||||
cur_node->last_lost_msg_count = num_lost;
|
||||
cur_node->last_pub_msg_count = num_msgs;
|
||||
cur_node = cur_node->next;
|
||||
}
|
||||
|
||||
start_time = current_time;
|
||||
|
||||
|
||||
PX4_INFO_RAW("\033[H"); // move cursor home and clear screen
|
||||
PX4_INFO_RAW(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics);
|
||||
#ifdef __PX4_NUTTX
|
||||
PX4_INFO_RAW(CLEAR_LINE "%*-s INST #SUB #MSG #LOST #QSIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME");
|
||||
#else
|
||||
PX4_INFO_RAW(CLEAR_LINE "%*s INST #SUB #MSG #LOST #QSIZE\n", -(int)max_topic_name_length + 2, "TOPIC NAME");
|
||||
#endif
|
||||
cur_node = first_node;
|
||||
|
||||
while (cur_node) {
|
||||
|
||||
if (!print_active_only || cur_node->pub_msg_delta > 0) {
|
||||
#ifdef __PX4_NUTTX
|
||||
PX4_INFO_RAW(CLEAR_LINE "%*-s %2i %4i %4i %5i %i\n", (int)max_topic_name_length,
|
||||
#else
|
||||
PX4_INFO_RAW(CLEAR_LINE "%*s %2i %4i %4i %5i %i\n", -(int)max_topic_name_length,
|
||||
#endif
|
||||
cur_node->node->get_meta()->o_name, (int)cur_node->instance,
|
||||
(int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta,
|
||||
(int)cur_node->lost_msg_delta, cur_node->node->get_queue_size());
|
||||
}
|
||||
|
||||
cur_node = cur_node->next;
|
||||
}
|
||||
|
||||
lock();
|
||||
addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters);
|
||||
unlock();
|
||||
}
|
||||
}
|
||||
|
||||
//cleanup
|
||||
cur_node = first_node;
|
||||
|
||||
while (cur_node) {
|
||||
DeviceNodeStatisticsData *next_node = cur_node->next;
|
||||
delete cur_node;
|
||||
cur_node = next_node;
|
||||
}
|
||||
}
|
||||
|
||||
#undef CLEAR_LINE
|
||||
|
||||
uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
|
||||
{
|
||||
lock();
|
||||
uORB::DeviceNode *node = getDeviceNodeLocked(nodepath);
|
||||
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 *rc = nullptr;
|
||||
|
||||
if (_node_map.find(nodepath)) {
|
||||
rc = _node_map.get(nodepath);
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
#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
|
||||
123
src/modules/uORB/uORBDeviceMaster.hpp
Normal file
123
src/modules/uORB/uORBDeviceMaster.hpp
Normal file
@@ -0,0 +1,123 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2016 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "uORBCommon.hpp"
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
class DeviceNode;
|
||||
class DeviceMaster;
|
||||
class Manager;
|
||||
}
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include "ORBMap.hpp"
|
||||
|
||||
#else
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
|
||||
#endif /* __PX4_NUTTX */
|
||||
|
||||
/**
|
||||
* Master control device for ObjDev.
|
||||
*
|
||||
* Used primarily to create new objects via the ORBIOCCREATE
|
||||
* ioctl.
|
||||
*/
|
||||
class uORB::DeviceMaster : public device::CDev
|
||||
{
|
||||
public:
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Public interface for getDeviceNodeLocked(). Takes care of synchronization.
|
||||
* @return node if exists, nullptr otherwise
|
||||
*/
|
||||
uORB::DeviceNode *getDeviceNode(const char *node_name);
|
||||
|
||||
/**
|
||||
* Print statistics for each existing topic.
|
||||
* @param reset if true, reset statistics afterwards
|
||||
*/
|
||||
void printStatistics(bool reset);
|
||||
|
||||
/**
|
||||
* Continuously print statistics, like the unix top command for processes.
|
||||
* Exited when the user presses the enter key.
|
||||
* @param topic_filter list of topic filters: if set, each string can be a substring for topics to match.
|
||||
* Or it can be '-a', which means to print all topics instead of only currently publishing ones.
|
||||
* @param num_filters
|
||||
*/
|
||||
void showTop(char **topic_filter, int num_filters);
|
||||
|
||||
private:
|
||||
// Private constructor, uORB::Manager takes care of its creation
|
||||
DeviceMaster();
|
||||
virtual ~DeviceMaster() = default;
|
||||
|
||||
struct DeviceNodeStatisticsData {
|
||||
DeviceNode *node;
|
||||
uint8_t instance;
|
||||
uint32_t last_lost_msg_count;
|
||||
unsigned int last_pub_msg_count;
|
||||
uint32_t lost_msg_delta;
|
||||
unsigned int pub_msg_delta;
|
||||
DeviceNodeStatisticsData *next = nullptr;
|
||||
};
|
||||
|
||||
void addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, size_t &max_topic_name_length,
|
||||
char **topic_filter, int num_filters);
|
||||
|
||||
friend class uORB::Manager;
|
||||
|
||||
/**
|
||||
* Find a node give its name.
|
||||
* _lock must already be held when calling this.
|
||||
* @return node if exists, nullptr otherwise
|
||||
*/
|
||||
uORB::DeviceNode *getDeviceNodeLocked(const char *node_name);
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
ORBMap _node_map;
|
||||
#else
|
||||
std::map<std::string, uORB::DeviceNode *> _node_map;
|
||||
#endif
|
||||
hrt_abstime _last_statistics_output;
|
||||
};
|
||||
@@ -31,36 +31,17 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <poll.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include "uORBDeviceNode.hpp"
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#define FILE_FLAGS(filp) filp->f_oflags
|
||||
#define FILE_PRIV(filp) filp->f_priv
|
||||
#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 FILE_FLAGS(filp) filp->flags
|
||||
#define FILE_PRIV(filp) filp->priv
|
||||
#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
|
||||
|
||||
#include "uORBDevices.hpp"
|
||||
#include "uORBDeviceNode.hpp"
|
||||
#include "uORBUtils.hpp"
|
||||
#include "uORBManager.hpp"
|
||||
|
||||
@@ -68,9 +49,6 @@
|
||||
#include "uORBCommunicator.hpp"
|
||||
#endif /* ORB_COMMUNICATOR */
|
||||
|
||||
#include <px4_sem.hpp>
|
||||
#include <stdlib.h>
|
||||
|
||||
using namespace device;
|
||||
|
||||
uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp)
|
||||
@@ -99,7 +77,6 @@ uORB::DeviceNode::~DeviceNode()
|
||||
if (_data != nullptr) {
|
||||
delete[] _data;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int
|
||||
@@ -826,401 +803,3 @@ int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
|
||||
_queue_size = queue_size;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
uORB::DeviceMaster::DeviceMaster() :
|
||||
CDev("obj_master", TOPIC_MASTER_DEVICE_PATH)
|
||||
{
|
||||
_last_statistics_output = hrt_absolute_time();
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret;
|
||||
|
||||
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];
|
||||
|
||||
/* construct a path to the node - this also checks the node name */
|
||||
ret = uORB::Utils::node_mkpath(nodepath, meta, adv->instance);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = PX4_ERROR;
|
||||
|
||||
/* try for topic groups */
|
||||
const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
|
||||
unsigned group_tries = 0;
|
||||
|
||||
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;
|
||||
|
||||
if (group_tries >= max_group_tries) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
}
|
||||
|
||||
SmartLock smart_lock(_lock);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
const char *objname = meta->o_name; //no need for a copy, meta->o_name will never be freed or changed
|
||||
|
||||
/* 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, objname, 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;
|
||||
}
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
void uORB::DeviceMaster::printStatistics(bool reset)
|
||||
{
|
||||
hrt_abstime current_time = hrt_absolute_time();
|
||||
PX4_INFO("Statistics, since last output (%i ms):",
|
||||
(int)((current_time - _last_statistics_output) / 1000));
|
||||
_last_statistics_output = current_time;
|
||||
|
||||
PX4_INFO("TOPIC, NR LOST MSGS");
|
||||
bool had_print = false;
|
||||
|
||||
lock();
|
||||
ITERATE_NODE_MAP() {
|
||||
INIT_NODE_MAP_VARS(node, node_name)
|
||||
|
||||
if (node->print_statistics(reset)) {
|
||||
had_print = true;
|
||||
}
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
if (!had_print) {
|
||||
PX4_INFO("No lost messages");
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
num_topics = 0;
|
||||
DeviceNodeStatisticsData *last_node = *first_node;
|
||||
|
||||
if (last_node) {
|
||||
while (last_node->next) {
|
||||
last_node = last_node->next;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
ITERATE_NODE_MAP() {
|
||||
INIT_NODE_MAP_VARS(node, node_name)
|
||||
++num_topics;
|
||||
|
||||
//check if already added
|
||||
cur_node = *first_node;
|
||||
|
||||
while (cur_node && cur_node->node != node) {
|
||||
cur_node = cur_node->next;
|
||||
}
|
||||
|
||||
if (cur_node) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (num_filters > 0 && topic_filter) {
|
||||
bool matched = false;
|
||||
|
||||
for (int i = 0; i < num_filters; ++i) {
|
||||
if (strstr(node->get_meta()->o_name, topic_filter[i])) {
|
||||
matched = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!matched) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (last_node) {
|
||||
last_node->next = new DeviceNodeStatisticsData();
|
||||
last_node = last_node->next;
|
||||
|
||||
} else {
|
||||
*first_node = last_node = new DeviceNodeStatisticsData();
|
||||
}
|
||||
|
||||
if (!last_node) {
|
||||
PX4_ERR("mem alloc failed");
|
||||
break;
|
||||
}
|
||||
|
||||
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) {
|
||||
max_topic_name_length = name_length;
|
||||
}
|
||||
|
||||
last_node->last_lost_msg_count = last_node->node->lost_message_count();
|
||||
last_node->last_pub_msg_count = last_node->node->published_message_count();
|
||||
}
|
||||
}
|
||||
|
||||
#define CLEAR_LINE "\033[K"
|
||||
|
||||
void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
|
||||
{
|
||||
|
||||
bool print_active_only = true;
|
||||
|
||||
if (topic_filter && num_filters > 0) {
|
||||
if (!strcmp("-a", topic_filter[0])) {
|
||||
num_filters = 0;
|
||||
}
|
||||
|
||||
print_active_only = false; // print non-active if -a or some filter given
|
||||
}
|
||||
|
||||
PX4_INFO_RAW("\033[2J\n"); //clear screen
|
||||
|
||||
lock();
|
||||
|
||||
if (_node_map.empty()) {
|
||||
unlock();
|
||||
PX4_INFO("no active topics");
|
||||
return;
|
||||
}
|
||||
|
||||
DeviceNodeStatisticsData *first_node = nullptr;
|
||||
DeviceNodeStatisticsData *cur_node = nullptr;
|
||||
size_t max_topic_name_length = 0;
|
||||
int num_topics = 0;
|
||||
addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters);
|
||||
|
||||
/* a DeviceNode is never deleted, so it's save to unlock here and still access the DeviceNodes */
|
||||
unlock();
|
||||
|
||||
#ifdef __PX4_QURT //QuRT has no poll()
|
||||
int num_runs = 0;
|
||||
#else
|
||||
const int stdin_fileno = 0;
|
||||
|
||||
struct pollfd fds;
|
||||
fds.fd = stdin_fileno;
|
||||
fds.events = POLLIN;
|
||||
#endif
|
||||
bool quit = false;
|
||||
|
||||
hrt_abstime start_time = hrt_absolute_time();
|
||||
|
||||
while (!quit) {
|
||||
|
||||
#ifdef __PX4_QURT
|
||||
|
||||
if (++num_runs > 1) {
|
||||
quit = true; //just exit after one output
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
/* Sleep 200 ms waiting for user input five times ~ 1s */
|
||||
for (int k = 0; k < 5; k++) {
|
||||
char c;
|
||||
|
||||
int ret = ::poll(&fds, 1, 0); //just want to check if there is new data available
|
||||
|
||||
if (ret > 0) {
|
||||
|
||||
ret = ::read(stdin_fileno, &c, 1);
|
||||
|
||||
if (ret) {
|
||||
quit = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
usleep(200000);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
if (!quit) {
|
||||
|
||||
//update the stats
|
||||
hrt_abstime current_time = hrt_absolute_time();
|
||||
float dt = (current_time - start_time) / 1.e6f;
|
||||
cur_node = first_node;
|
||||
|
||||
while (cur_node) {
|
||||
uint32_t num_lost = cur_node->node->lost_message_count();
|
||||
unsigned int num_msgs = cur_node->node->published_message_count();
|
||||
cur_node->pub_msg_delta = (num_msgs - cur_node->last_pub_msg_count) / dt;
|
||||
cur_node->lost_msg_delta = (num_lost - cur_node->last_lost_msg_count) / dt;
|
||||
cur_node->last_lost_msg_count = num_lost;
|
||||
cur_node->last_pub_msg_count = num_msgs;
|
||||
cur_node = cur_node->next;
|
||||
}
|
||||
|
||||
start_time = current_time;
|
||||
|
||||
|
||||
PX4_INFO_RAW("\033[H"); // move cursor home and clear screen
|
||||
PX4_INFO_RAW(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics);
|
||||
#ifdef __PX4_NUTTX
|
||||
PX4_INFO_RAW(CLEAR_LINE "%*-s INST #SUB #MSG #LOST #QSIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME");
|
||||
#else
|
||||
PX4_INFO_RAW(CLEAR_LINE "%*s INST #SUB #MSG #LOST #QSIZE\n", -(int)max_topic_name_length + 2, "TOPIC NAME");
|
||||
#endif
|
||||
cur_node = first_node;
|
||||
|
||||
while (cur_node) {
|
||||
|
||||
if (!print_active_only || cur_node->pub_msg_delta > 0) {
|
||||
#ifdef __PX4_NUTTX
|
||||
PX4_INFO_RAW(CLEAR_LINE "%*-s %2i %4i %4i %5i %i\n", (int)max_topic_name_length,
|
||||
#else
|
||||
PX4_INFO_RAW(CLEAR_LINE "%*s %2i %4i %4i %5i %i\n", -(int)max_topic_name_length,
|
||||
#endif
|
||||
cur_node->node->get_meta()->o_name, (int)cur_node->instance,
|
||||
(int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta,
|
||||
(int)cur_node->lost_msg_delta, cur_node->node->get_queue_size());
|
||||
}
|
||||
|
||||
cur_node = cur_node->next;
|
||||
}
|
||||
|
||||
lock();
|
||||
addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters);
|
||||
unlock();
|
||||
}
|
||||
}
|
||||
|
||||
//cleanup
|
||||
cur_node = first_node;
|
||||
|
||||
while (cur_node) {
|
||||
DeviceNodeStatisticsData *next_node = cur_node->next;
|
||||
delete cur_node;
|
||||
cur_node = next_node;
|
||||
}
|
||||
}
|
||||
|
||||
#undef CLEAR_LINE
|
||||
|
||||
uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
|
||||
{
|
||||
lock();
|
||||
uORB::DeviceNode *node = getDeviceNodeLocked(nodepath);
|
||||
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 *rc = nullptr;
|
||||
|
||||
if (_node_map.find(nodepath)) {
|
||||
rc = _node_map.get(nodepath);
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
#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
|
||||
@@ -33,21 +33,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "uORBCommon.hpp"
|
||||
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include "ORBMap.hpp"
|
||||
|
||||
#else
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
|
||||
#endif /* __PX4_NUTTX */
|
||||
#include "uORBDeviceMaster.hpp"
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
@@ -265,69 +252,3 @@ private:
|
||||
DeviceNode(const DeviceNode &);
|
||||
DeviceNode &operator=(const DeviceNode &);
|
||||
};
|
||||
|
||||
/**
|
||||
* Master control device for ObjDev.
|
||||
*
|
||||
* Used primarily to create new objects via the ORBIOCCREATE
|
||||
* ioctl.
|
||||
*/
|
||||
class uORB::DeviceMaster : public device::CDev
|
||||
{
|
||||
public:
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Public interface for getDeviceNodeLocked(). Takes care of synchronization.
|
||||
* @return node if exists, nullptr otherwise
|
||||
*/
|
||||
uORB::DeviceNode *getDeviceNode(const char *node_name);
|
||||
|
||||
/**
|
||||
* Print statistics for each existing topic.
|
||||
* @param reset if true, reset statistics afterwards
|
||||
*/
|
||||
void printStatistics(bool reset);
|
||||
|
||||
/**
|
||||
* Continuously print statistics, like the unix top command for processes.
|
||||
* Exited when the user presses the enter key.
|
||||
* @param topic_filter list of topic filters: if set, each string can be a substring for topics to match.
|
||||
* Or it can be '-a', which means to print all topics instead of only currently publishing ones.
|
||||
* @param num_filters
|
||||
*/
|
||||
void showTop(char **topic_filter, int num_filters);
|
||||
|
||||
private:
|
||||
// Private constructor, uORB::Manager takes care of its creation
|
||||
DeviceMaster();
|
||||
virtual ~DeviceMaster() = default;
|
||||
|
||||
struct DeviceNodeStatisticsData {
|
||||
DeviceNode *node;
|
||||
uint8_t instance;
|
||||
uint32_t last_lost_msg_count;
|
||||
unsigned int last_pub_msg_count;
|
||||
uint32_t lost_msg_delta;
|
||||
unsigned int pub_msg_delta;
|
||||
DeviceNodeStatisticsData *next = nullptr;
|
||||
};
|
||||
void addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, size_t &max_topic_name_length,
|
||||
char **topic_filter, int num_filters);
|
||||
|
||||
friend class uORB::Manager;
|
||||
|
||||
/**
|
||||
* Find a node give its name.
|
||||
* _lock must already be held when calling this.
|
||||
* @return node if exists, nullptr otherwise
|
||||
*/
|
||||
uORB::DeviceNode *getDeviceNodeLocked(const char *node_name);
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
ORBMap _node_map;
|
||||
#else
|
||||
std::map<std::string, uORB::DeviceNode *> _node_map;
|
||||
#endif
|
||||
hrt_abstime _last_statistics_output;
|
||||
};
|
||||
@@ -32,10 +32,11 @@
|
||||
****************************************************************************/
|
||||
|
||||
#include <string.h>
|
||||
#include "uORBDevices.hpp"
|
||||
|
||||
#include "uORBManager.hpp"
|
||||
#include "uORB.h"
|
||||
#include "uORBCommon.hpp"
|
||||
|
||||
#include <px4_log.h>
|
||||
#include <px4_module.h>
|
||||
|
||||
|
||||
@@ -35,15 +35,14 @@
|
||||
#include <sys/stat.h>
|
||||
#include <stdarg.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_posix.h>
|
||||
#include <px4_tasks.h>
|
||||
|
||||
#include "uORBDeviceNode.hpp"
|
||||
#include "uORBUtils.hpp"
|
||||
#include "uORBManager.hpp"
|
||||
#include "uORBDevices.hpp"
|
||||
|
||||
|
||||
//========================= Static initializations =================
|
||||
|
||||
@@ -35,8 +35,10 @@
|
||||
#define _uORBManager_hpp_
|
||||
|
||||
#include "uORBCommon.hpp"
|
||||
#include "uORBDevices.hpp"
|
||||
#include "uORBDeviceMaster.hpp"
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#include "ORBSet.hpp"
|
||||
#else
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
#include <string.h>
|
||||
#include "../uORBDevices.hpp"
|
||||
#include "../uORBDeviceNode.hpp"
|
||||
#include "../uORB.h"
|
||||
#include "../uORBCommon.hpp"
|
||||
|
||||
|
||||
Reference in New Issue
Block a user