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

microRTPS_client: use updated uORB API; improve usage

This commit is contained in:
TSC21 2019-11-22 19:54:59 +00:00 committed by Nuno Marques
parent bc182e94e6
commit ec0803815e
3 changed files with 20 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
msg/templates/uorb_microcdr/microRTPS_client.cpp.em
View File

@ -69,6 +69,8 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] =
#include <px4_time.h>
#include <uORB/uORB.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
@[for topic in list(set(topic_names))]@
#include <uORB/topics/@(topic).h>
#include <uORB_microcdr/topics/@(topic).h>
@ -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;

6
src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
View File

@ -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;

19
src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
View File

@ -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");