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

refactor uxrce_dds_client: reduce amount of generated code for SendTopicsSubs 

Reduces flash usage by ~4KB for current set of topics.
This commit is contained in:
Beat Küng 2023-08-07 14:42:20 +02:00 committed by Daniel Agar
parent 8e1dd710a6
commit d4c7ea74ef
2 changed files with 40 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/msg/templates/ucdr/msg.h.em
View File

@ -124,8 +124,9 @@ static inline constexpr int ucdr_topic_size_@(topic)()
return @(struct_size);
}
bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf, int64_t time_offset = 0)
bool ucdr_serialize_@(topic)(const void* data, ucdrBuffer& buf, int64_t time_offset = 0)
{
const @(uorb_struct)& topic = *static_cast<const @(uorb_struct)*>(data);
@{
for field_type, field_name, field_size, padding in fields:
if padding > 0:

57
src/modules/uxrce_dds_client/dds_topics.h.em
View File

@ -27,12 +27,33 @@ import os
#include <uORB/ucdr/@(include).h>
@[end for]@
typedef bool (*UcdrSerializeMethod)(const void* data, ucdrBuffer& buf, int64_t time_offset);
static constexpr int max_topic_size = 512;
@[ for pub in publications]@
static_assert(sizeof(@(pub['simple_base_type'])_s) <= max_topic_size, "topic too large, increase max_topic_size");
@[ end for]@
struct SendSubscription {
uORB::Subscription subscription;
uxrObjectId data_writer;
const char* dds_type_name;
uint32_t topic_size;
UcdrSerializeMethod ucdr_serialize_method;
};
// Subscribers for messages to send
struct SendTopicsSubs {
SendSubscription send_subscriptions[@(len(publications))] = {
@[ for pub in publications]@
uORB::Subscription @(pub['topic_simple'])_sub{ORB_ID(@(pub['topic_simple']))};
uxrObjectId @(pub['topic_simple'])_data_writer{};
{ uORB::Subscription(ORB_ID(@(pub['topic_simple']))),
uxr_object_id(0, UXR_INVALID_ID),
"@(pub['dds_type'])",
ucdr_topic_size_@(pub['simple_base_type'])(),
&ucdr_serialize_@(pub['simple_base_type']),
},
@[ end for]@
};
uint32_t num_payload_sent{};
@ -42,47 +63,45 @@ struct SendTopicsSubs {
void SendTopicsSubs::reset() {
num_payload_sent = 0;
@[ for idx, pub in enumerate(publications)]@
@(pub['topic_simple'])_data_writer = uxr_object_id(0, UXR_INVALID_ID);
@[ end for]@
for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) {
send_subscriptions[idx].data_writer = uxr_object_id(0, UXR_INVALID_ID);
}
};
void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace)
{
int64_t time_offset_us = session->time_offset / 1000; // ns -> us
@[ for idx, pub in enumerate(publications)]@
{
@(pub['simple_base_type'])_s data;
alignas(sizeof(uint64_t)) char topic_data[max_topic_size];
if (@(pub['topic_simple'])_sub.update(&data)) {
if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) {
for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) {
if (send_subscriptions[idx].subscription.update(&topic_data)) {
if (send_subscriptions[idx].data_writer.id == UXR_INVALID_ID) {
// data writer not created yet
create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer);
create_data_writer(session, reliable_out_stream_id, participant_id, send_subscriptions[idx].subscription.orb_id(), client_namespace, send_subscriptions[idx].subscription.get_topic()->o_name,
send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer);
}
if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) {
if (send_subscriptions[idx].data_writer.id != UXR_INVALID_ID) {
ucdrBuffer ub;
uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])();
if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) {
ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us);
uint32_t topic_size = send_subscriptions[idx].topic_size;
if (uxr_prepare_output_stream(session, best_effort_stream_id, send_subscriptions[idx].data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) {
send_subscriptions[idx].ucdr_serialize_method(&topic_data, ub, time_offset_us);
// TODO: fill up the MTU and then flush, which reduces the packet overhead
uxr_flash_output_streams(session);
num_payload_sent += topic_size;
} else {
//PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])");
//PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name);
}
} else {
//PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])");
//PX4_ERR("Error UXR_INVALID_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name);
}
}
}
@[ end for]@
}
// Publishers for received messages