Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 18:27:05 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

microRTPS: client: use structs for pub/subs to avoid increasing the stack usage

Browse Source
This commit is contained in:
TSC21
2020-07-28 12:00:39 +01:00
committed by Nuno Marques
parent 3db9307dfb
commit 311a8144e1
1 changed files with 46 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/uorb_microcdr/microRTPS_client.cpp.em
+46 -31
View File
@@ -81,6 +81,27 @@ void* send(void *data);
uint8_t last_remote_msg_seq = 0;
uint8_t last_msg_seq = 0;
@[if recv_topics]@
// Publishers for received messages
struct RcvTopicsPubs
{
@[ for idx, topic in enumerate(recv_topics)]@
uORB::Publication<@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))};
@[ end for]@
};
@[end if]@
@[if send_topics]@
// Subscribers for messages to send
struct SendTopicsSubs
{
@[ for idx, topic in enumerate(send_topics)]@
uORB::Subscription @(topic)_sub{ORB_ID(@(topic))};
@[ end for]@
};
@[end if]@
@[if send_topics]@
void* send(void* /*unused*/)
{
@@ -89,11 +110,7 @@ void* send(void* /*unused*/)
int loop = 0, read = 0;
uint32_t length = 0;
size_t header_length = 0;
/* subscribe to topics */
@[for idx, topic in enumerate(send_topics)]@
uORB::Subscription @(topic)_sub{ORB_ID(@(topic))};
@[end for]@
struct SendTopicsSubs subs;
// ucdrBuffer to serialize using the user defined buffer
ucdrBuffer writer;
@@ -106,30 +123,32 @@ void* send(void* /*unused*/)
while (!_should_exit_task)
{
@[for idx, topic in enumerate(send_topics)]@
@(send_base_types[idx])_s @(topic)_data;
if (@(topic)_sub.update(&@(topic)_data)) {
{
@(send_base_types[idx])_s @(topic)_data;
if (subs.@(topic)_sub.update(&@(topic)_data)) {
@[if topic == 'Timesync' or topic == 'timesync']@
if(@(topic)_data.sys_id == 0 && @(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) {
last_remote_msg_seq = @(topic)_data.seq;
if(@(topic)_data.sys_id == 0 && @(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) {
last_remote_msg_seq = @(topic)_data.seq;
@(topic)_data.timestamp = hrt_absolute_time();
@(topic)_data.sys_id = 1;
@(topic)_data.seq = last_msg_seq;
@(topic)_data.tc1 = hrt_absolute_time() * 1000ULL;
@(topic)_data.ts1 = @(topic)_data.ts1;
@(topic)_data.timestamp = hrt_absolute_time();
@(topic)_data.sys_id = 1;
@(topic)_data.seq = last_msg_seq;
@(topic)_data.tc1 = hrt_absolute_time() * 1000ULL;
@(topic)_data.ts1 = @(topic)_data.ts1;
last_msg_seq++;
last_msg_seq++;
@[end if]@
// copy raw data into local buffer. Payload is shifted by header length to make room for header
serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length);
if (0 < (read = transport_node->write(static_cast<char>(@(rtps_message_id(ids, topic))), data_buffer, length)))
{
total_sent += read;
++sent;
// copy raw data into local buffer. Payload is shifted by header length to make room for header
serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length);
if (0 < (read = transport_node->write(static_cast<char>(@(rtps_message_id(ids, topic))), data_buffer, length)))
{
total_sent += read;
++sent;
}
@[if topic == 'Timesync' or topic == 'timesync']@
}
@[if topic == 'Timesync' or topic == 'timesync']@
}
@[end if]@
}
}
@[end for]@
px4_usleep(_options.sleep_ms * 1000);
@@ -149,7 +168,7 @@ static int launch_send_thread(pthread_t &sender_thread)
{
pthread_attr_t sender_thread_attr;
pthread_attr_init(&sender_thread_attr);
pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(4000));
pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(4096));
struct sched_param param;
(void)pthread_attr_getschedparam(&sender_thread_attr, &param);
param.sched_priority = SCHED_PRIORITY_DEFAULT;
@@ -168,12 +187,7 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64
char data_buffer[BUFFER_SIZE] = {};
int read = 0;
uint8_t topic_ID = 255;
// Declare received topics
@[for idx, topic in enumerate(recv_topics)]@
@(receive_base_types[idx])_s @(topic)_data;
uORB::Publication<@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))};
@[end for]@
struct RcvTopicsPubs pubs;
// ucdrBuffer to deserialize using the user defined buffer
ucdrBuffer reader;
@@ -200,8 +214,9 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64
@[for idx, topic in enumerate(recv_topics)]@
case @(rtps_message_id(ids, topic)):
{
@(receive_base_types[idx])_s @(topic)_data;
deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer);
@(topic)_pub.publish(@(topic)_data);
pubs.@(topic)_pub.publish(@(topic)_data);
++received;
}
break;