mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-30 16:10:36 +08:00
uORB advertise through uORBDeviceMaster directly
This commit is contained in:
Daniel Agar
@@ -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) {
|
||||
|
||||
Reference in New Issue
Block a user