fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 02:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixes to the ModalAI muorb implementation

Browse Source
This commit is contained in:
Eric Katzfey
2024-02-27 19:56:17 -08:00
parent b405d75553
commit 76352765b6
3 changed files with 52 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/common/uORB/uORBManager.cpp
+26 -24
View File
@@ -602,6 +602,22 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
{
PX4_DEBUG("entering process_remote_topic: name: %s", topic_name);
// First make sure this is a valid topic
const struct orb_metadata *const *topic_list = orb_get_topics();
orb_id_t topic_ptr = nullptr;
for (size_t i = 0; i < orb_topics_count(); i++) {
if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
topic_ptr = topic_list[i];
break;
}
}
if (! topic_ptr) {
PX4_ERR("process_remote_topic meta not found for %s\n", topic_name);
return -1;
}
// Look to see if we already have a node for this topic
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, topic_name);
@@ -613,7 +629,8 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
if (node) {
PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
PX4_DEBUG("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
node->update_queue_size(topic_ptr->o_queue);
node->mark_as_advertised();
_remote_topics.insert(topic_name);
return 0;
@@ -622,27 +639,9 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
}
// We didn't find a node so we need to create it via an advertisement
const struct orb_metadata *const *topic_list = orb_get_topics();
orb_id_t topic_ptr = nullptr;
for (size_t i = 0; i < orb_topics_count(); i++) {
if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
topic_ptr = topic_list[i];
break;
}
}
if (topic_ptr) {
PX4_INFO("Advertising remote topic %s", topic_name);
_remote_topics.insert(topic_name);
// Add some queue depth when advertising remote topics. These
// topics may get aggregated and thus delivered in a batch that
// requires some buffering in a queue.
orb_advertise(topic_ptr, nullptr, 5);
} else {
PX4_INFO("process_remote_topic meta not found for %s\n", topic_name);
}
PX4_DEBUG("Advertising remote topic %s", topic_name);
_remote_topics.insert(topic_name);
orb_advertise(topic_ptr, nullptr, topic_ptr->o_queue);
return 0;
}
@@ -663,8 +662,11 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName)
PX4_DEBUG("DeviceNode(%s) not created yet", messageName);
} else {
// node is present.
node->process_add_subscription();
// node is present. But don't send any data to it if it
// is a node advertised by the remote side
if (_remote_topics.find(messageName) == false) {
node->process_add_subscription();
}
}
} else {