diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em index d8d32ab2b2..34b5566101 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em @@ -69,6 +69,8 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] = #include #include +#include +#include @[for topic in list(set(topic_names))]@ #include #include @@ -86,12 +88,8 @@ void* send(void* /*unused*/) uint16_t header_length = 0; /* subscribe to topics */ - int fds[@(len(send_topics))] = {}; - - // orb_set_interval statblish an update interval period in milliseconds. @[for idx, topic in enumerate(send_topics)]@ - fds[@(idx)] = orb_subscribe(ORB_ID(@(topic))); - orb_set_interval(fds[@(idx)], _options.update_time_ms); + uORB::Subscription @(topic)_sub{ORB_ID(@(topic))}; @[end for]@ // ucdrBuffer to serialize using the user defined buffer @@ -104,24 +102,18 @@ void* send(void* /*unused*/) while (!_should_exit_task) { - bool updated; @[for idx, topic in enumerate(send_topics)]@ - orb_check(fds[@(idx)], &updated); - if (updated) - { - // obtained data for the file descriptor - struct @(send_base_types[idx])_s data; - // copy raw data into local buffer - if (orb_copy(ORB_ID(@(topic)), fds[@(idx)], &data) == 0) { - /* payload is shifted by header length to make room for header*/ - serialize_@(send_base_types[idx])(&writer, &data, &data_buffer[header_length], &length); + @(send_base_types[idx])_s @(topic)_data; + if (@(topic)_sub.update(&@(topic)_data)) { + // 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((char)@(rtps_message_id(ids, topic)), data_buffer, length))) { total_sent += read; ++sent; } - } } @[end for]@ @@ -164,8 +156,8 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r // Declare received topics @[for idx, topic in enumerate(recv_topics)]@ - struct @(receive_base_types[idx])_s @(topic)_data; - orb_advert_t @(topic)_pub = nullptr; + @(receive_base_types[idx])_s @(topic)_data; + uORB::Publication<@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))}; @[end for]@ // ucdrBuffer to deserialize using the user defined buffer @@ -194,11 +186,7 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r case @(rtps_message_id(ids, topic)): { deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer); - if (!@(topic)_pub) { - @(topic)_pub = orb_advertise(ORB_ID(@(topic)), &@(topic)_data); - } else { - orb_publish(ORB_ID(@(topic)), @(topic)_pub, &@(topic)_data); - } + @(topic)_pub.publish(@(topic)_data); ++received; } break; diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h index 11391345f4..71a45cdc85 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -74,11 +74,11 @@ struct options { }; eTransports transport = options::eTransports::UART; char device[64] = DEVICE; - int update_time_ms = UPDATE_TIME_MS; + uint32_t update_time_ms = UPDATE_TIME_MS; int loops = LOOPS; - int sleep_ms = SLEEP_MS; + uint32_t sleep_ms = SLEEP_MS; uint32_t baudrate = BAUDRATE; - int poll_ms = POLL_MS; + uint32_t poll_ms = POLL_MS; char ip[16] = IP; uint16_t recv_port = DEFAULT_RECV_PORT; uint16_t send_port = DEFAULT_SEND_PORT; diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp index b70fa5b3c6..2dd58a505b 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -86,25 +86,23 @@ static int parse_options(int argc, char *argv[]) switch (ch) { case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ? options::eTransports::UDP - : options::eTransports::UART; break; + : options::eTransports::UART; break; - case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; + case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; - case 'u': _options.update_time_ms = strtol(myoptarg, nullptr, 10); break; + case 'u': _options.update_time_ms = strtoul(myoptarg, nullptr, 10); break; - case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; + case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; - case 'w': _options.sleep_ms = strtol(myoptarg, nullptr, 10); break; + case 'w': _options.sleep_ms = strtoul(myoptarg, nullptr, 10); break; case 'b': _options.baudrate = strtoul(myoptarg, nullptr, 10); break; - case 'p': _options.poll_ms = strtol(myoptarg, nullptr, 10); break; - case 'r': _options.recv_port = strtoul(myoptarg, nullptr, 10); break; case 's': _options.send_port = strtoul(myoptarg, nullptr, 10); break; - case 'i': if (nullptr != myoptarg) strcpy(_options.ip, myoptarg); break; + case 'i': if (nullptr != myoptarg) strcpy(_options.ip, myoptarg); break; default: usage(argv[1]); @@ -112,11 +110,6 @@ static int parse_options(int argc, char *argv[]) } } - if (_options.sleep_ms < 1) { - _options.sleep_ms = 1; - PX4_ERR("sleep time too low, using 1 ms"); - } - if (_options.poll_ms < 1) { _options.poll_ms = 1; PX4_ERR("poll timeout too low, using 1 ms");