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

Fixes to the ModalAI muorb implementation

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Tools/msg/templates/uorb/msg.cpp.em
View File

@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
@{
queue_length = 1
for constant in spec.constants:
if constant.name == 'ORB_QUEUE_LENGTH':
queue_length = constant.val
}@
@[for topic in topics]@
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic), @queue_length);
@[end for]
void print_message(const orb_metadata *meta, const @uorb_struct& message)

19
platforms/common/uORB/uORB.h
View File

@ -53,6 +53,10 @@ struct orb_metadata {
const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
uint32_t message_hash; /**< Hash over all fields for message compatibility checks */
orb_id_size_t o_id; /**< ORB_ID enum */
#ifdef CONFIG_ORB_COMMUNICATOR
uint8_t o_queue; /**< queue size */
#endif
};
typedef const struct orb_metadata *orb_id_t;
@ -102,8 +106,20 @@ typedef const struct orb_metadata *orb_id_t;
* @param _size_no_padding Struct size w/o padding at the end
* @param _message_hash 32 bit message hash over all fields
* @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status
* @param _queue_size Queue size for remote topics in communicator interface
*/
#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum) \
#ifdef CONFIG_ORB_COMMUNICATOR
#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum, _queue_size) \
const struct orb_metadata __orb_##_name = { \
#_name, \
sizeof(_struct), \
_size_no_padding, \
_fields, \
_orb_id_enum, \
_queue_size \
}; struct hack
#else
#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum, _queue_size) \
const struct orb_metadata __orb_##_name = { \
#_name, \
sizeof(_struct), \
@ -111,6 +127,7 @@ typedef const struct orb_metadata *orb_id_t;
_message_hash, \
_orb_id_enum \
}; struct hack
#endif
__BEGIN_DECLS

50
platforms/common/uORB/uORBManager.cpp
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 {