zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

microRTPS: add possibility to set HW or SW flow control; improve verbosity aesthetics

sbg
TSC21 5 years ago committed by Nuno Marques
parent
b2845c60d3
commit
2f4eff4c38
9 changed files with 158 additions and 125 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      msg/templates/urtps/Publisher.cpp.em
  2. 12
      msg/templates/urtps/RtpsTopics.cpp.em
  3. 4
      msg/templates/urtps/Subscriber.cpp.em
  4. 69
      msg/templates/urtps/microRTPS_agent.cpp.em
  5. 9
      msg/templates/urtps/microRTPS_timesync.cpp.em
  6. 148
      msg/templates/urtps/microRTPS_transport.cpp
  7. 4
      msg/templates/urtps/microRTPS_transport.h
  8. 2
      src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
  9. 31
      src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp

4
msg/templates/urtps/Publisher.cpp.em
Unescape View File

@ -144,10 +144,10 @@ void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub, Match @@ -144,10 +144,10 @@ void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub, Match
if (is_different_endpoint) {
if (info.status == MATCHED_MATCHING) {
n_matched++;
std::cout << " - @(topic) publisher matched" << std::endl;
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher matched\033[0m" << std::endl;
} else {
n_matched--;
std::cout << " - @(topic) publisher unmatched" << std::endl;
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher unmatched\033[0m" << std::endl;
}
}
}

12
msg/templates/urtps/RtpsTopics.cpp.em
Unescape View File

@ -60,7 +60,7 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se @@ -60,7 +60,7 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se
{
@[if recv_topics]@
// Initialise subscribers
std::cout << "--- Subscribers ---" << std::endl;
std::cout << "\033[0;36m--- Subscribers ---\033[0m" << std::endl;
@[for topic in recv_topics]@
if (_@(topic)_sub.init(@(rtps_message_id(ids, topic)), t_send_queue_cv, t_send_queue_mutex, t_send_queue)) {
std::cout << "- @(topic) subscriber started" << std::endl;
@ -69,11 +69,11 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se @@ -69,11 +69,11 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se
return false;
}
@[end for]@
std::cout << "--------------------" << std::endl << std::endl;
std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl;
@[end if]@
@[if send_topics]@
// Initialise publishers
std::cout << "---- Publishers ----" << std::endl;
std::cout << "\033[0;36m---- Publishers ----\033[0m" << std::endl;
@[for topic in send_topics]@
if (_@(topic)_pub.init()) {
std::cout << "- @(topic) publisher started" << std::endl;
@ -85,7 +85,7 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se @@ -85,7 +85,7 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se
return false;
}
@[end for]@
std::cout << "--------------------" << std::endl;
std::cout << "\033[0;36m-----------------------\033[0m" << std::endl;
@[end if]@
return true;
}
@ -119,7 +119,7 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len) @@ -119,7 +119,7 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
break;
@[end for]@
default:
printf("Unexpected topic ID to publish\n");
printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID to publish\033[0m\n");
break;
}
}
@ -153,7 +153,7 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr) @@ -153,7 +153,7 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
break;
@[end for]@
default:
printf("Unexpected topic ID '%hhu' to getMsg\n", topic_ID);
printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to getMsg\033[0m\n", topic_ID);
break;
}

4
msg/templates/urtps/Subscriber.cpp.em
Unescape View File

@ -147,10 +147,10 @@ void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, Ma @@ -147,10 +147,10 @@ void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, Ma
if (is_different_endpoint) {
if (info.status == MATCHED_MATCHING) {
n_matched++;
std::cout << " - @(topic) subscriber matched" << std::endl;
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber matched\033[0m" << std::endl;
} else {
n_matched--;
std::cout << " - @(topic) subscriber unmatched" << std::endl;
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber unmatched\033[0m" << std::endl;
}
}
@[else]@

69
msg/templates/urtps/microRTPS_agent.cpp.em
Unescape View File

@ -77,7 +77,7 @@ recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumer @@ -77,7 +77,7 @@ recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumer
#define DEVICE "/dev/ttyACM0"
#define SLEEP_US 1
#define BAUDRATE 460800
#define POLL_MS 0
#define POLL_MS 1
#define WAIT_CNST 2
#define DEFAULT_RECV_PORT 2020
#define DEFAULT_SEND_PORT 2019
@ -108,6 +108,8 @@ struct options { @@ -108,6 +108,8 @@ struct options {
uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_PORT;
char ip[16] = DEFAULT_IP;
bool sw_flow_control = false;
bool hw_flow_control = false;
} _options;
static void usage(const char *name)
@ -120,7 +122,9 @@ static void usage(const char *name) @@ -120,7 +122,9 @@ static void usage(const char *name)
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
" -r <reception port> UDP port for receiving. Default 2019\n"
" -s <sending port> UDP port for sending. Default 2020\n"
" -i <ip_address> Target IP for UDP. Default 127.0.0.1\n",
" -i <ip_address> Target IP for UDP. Default 127.0.0.1\n"
" -f <sw flow control> Activates UART link SW flow control\n"
" -h <hw flow control> Activates UART link HW flow control\n",
name);
}
@ -128,30 +132,36 @@ static int parse_options(int argc, char **argv) @@ -128,30 +132,36 @@ static int parse_options(int argc, char **argv)
{
int ch;
while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:")) != EOF)
while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:fh")) != EOF)
{
switch (ch)
{
case 't': _options.transport = strcmp(optarg, "UDP") == 0?
options::eTransports::UDP
:options::eTransports::UART; break;
case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break;
case 'w': _options.sleep_us = strtol(optarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break;
case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break;
case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break;
case 's': _options.send_port = strtoul(optarg, nullptr, 10); break;
case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break;
:options::eTransports::UART; break;
case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break;
case 'w': _options.sleep_us = strtol(optarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break;
case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break;
case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break;
case 's': _options.send_port = strtoul(optarg, nullptr, 10); break;
case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break;
case 'f': _options.sw_flow_control = true; break;
case 'h': _options.hw_flow_control = true; break;
default:
usage(argv[0]);
return -1;
return -1;
}
}
if (optind < argc)
{
usage(argv[0]);
return -1;
if (_options.poll_ms < 1) {
_options.poll_ms = 1;
printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too low, using 1 ms\033[0m");
}
if (_options.hw_flow_control && _options.sw_flow_control) {
printf("\033[0;31m[ micrortps_agent ]\tHW and SW flow control set. Please set only one or another\033[0m");
return -1;
}
return 0;
@ -159,7 +169,7 @@ static int parse_options(int argc, char **argv) @@ -159,7 +169,7 @@ static int parse_options(int argc, char **argv)
void signal_handler(int signum)
{
printf("Interrupt signal (%d) received.\n", signum);
printf("\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum);
running = 0;
transport_node->close();
timeSync->stop();
@ -209,40 +219,42 @@ int main(int argc, char** argv) @@ -209,40 +219,42 @@ int main(int argc, char** argv)
{
if (-1 == parse_options(argc, argv))
{
printf("EXITING...\n");
printf("\033[1;33m[ micrortps_agent ]\tEXITING...\033[0m\n");
return -1;
}
// register signal SIGINT and signal handler
signal(SIGINT, signal_handler);
printf("--- MicroRTPS Agent ---\n");
printf("- Starting link...\n");
printf("\033[0;37m--- MicroRTPS Agent ---\033[0m\n");
printf("[ micrortps_agent ]\tStarting link...\n");
switch (_options.transport)
{
case options::eTransports::UART:
{
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
printf("- UART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms\n",
_options.device, _options.baudrate, _options.sleep_us, _options.poll_ms);
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms,
_options.sw_flow_control, _options.hw_flow_control);
printf("[ micrortps_agent ]\tUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms; flow_control: %s\n",
_options.device, _options.baudrate, _options.sleep_us, _options.poll_ms,
_options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No"));
}
break;
case options::eTransports::UDP:
{
transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port);
printf("- UDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dus\n",
printf("[ micrortps_agent ]\tUDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dus\n",
_options.ip, _options.recv_port, _options.send_port, _options.sleep_us);
}
break;
default:
printf("EXITING...\n");
printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n");
return -1;
}
if (0 > transport_node->init())
{
printf("EXITING...\n");
printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n");
return -1;
}
@ -287,9 +299,8 @@ int main(int argc, char** argv) @@ -287,9 +299,8 @@ int main(int argc, char** argv)
(!running && loop > 1))
{
std::chrono::duration<double> elapsed_secs = end - start;
printf("\nSENT: %lu messages - %lu bytes\n",
(unsigned long)sent, (unsigned long)total_sent);
printf("RECEIVED: %d messages - %d bytes; %d LOOPS - %.03f seconds - %.02fKB/s\n\n",
printf("[ micrortps_agent ]\tSENT: %lumessages \t- %lubytes\n", (unsigned long)sent, (unsigned long)total_sent);
printf("[ micrortps_agent ]\tRECEIVED: %dmessages \t- %dbytes; %d LOOPS - %.03f seconds - %.02fKB/s\n",
received, total_read, loop, elapsed_secs.count(), (double)total_read/(1000*elapsed_secs.count()));
received = sent = total_read = total_sent = 0;
receiving = false;

9
msg/templates/urtps/microRTPS_timesync.cpp.em
Unescape View File

@ -116,7 +116,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t @@ -116,7 +116,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
if (_request_reset_counter > REQUEST_RESET_COUNTER_THRESHOLD) {
reset();
std::cout << std::endl << "Timesync clock changed, resetting" << std::endl;
std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl;
}
if (_num_samples == 0) {
@ -127,7 +127,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t @@ -127,7 +127,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
if (_num_samples >= WINDOW_SIZE) {
if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) {
_request_reset_counter++;
std::cout << std::endl << "Timesync offset outlier, discarding" << std::endl;
std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl;
return false;
} else {
_request_reset_counter = 0;
@ -136,8 +136,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t @@ -136,8 +136,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
// ignore if rtti > 10ms
if (rtti > 15ll * 1000ll * 1000ll) {
std::cout << std::endl
<< "RTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms" << std::endl;
std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms\033[0m" << std::endl;
return false;
}
@ -168,7 +167,7 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) { @@ -168,7 +167,7 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
if (getMsgTC1(msg) > 0) {
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getMonoRawTimeNSec())) {
std::cerr << "Offset not updated" << std::endl;
std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl;
}
} else if (getMsgTC1(msg) == 0) {

148
msg/templates/urtps/microRTPS_transport.cpp
Unescape View File

@ -118,7 +118,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer @@ -118,7 +118,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) {
#ifndef PX4_ERR
printf("Read fail %d\n", errsv);
printf("\033[0;31m[ micrortps_transport ]\tRead fail %d\033[0m\n", errsv);
#else
PX4_ERR("Read fail %d", errsv);
#endif /* PX4_ERR */
@ -147,11 +147,11 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer @@ -147,11 +147,11 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
// Start not found
if (msg_start_pos > rx_buff_pos - header_size) {
#ifndef PX4_INFO
printf(" (↓↓ %u)\n", msg_start_pos);
#ifndef PX4_WARN
printf("\033[1;33m[ micrortps_transport ]\t (↓↓ %u)\033[0m\n", msg_start_pos);
#else
PX4_INFO(" (↓↓ %u)", msg_start_pos);
#endif /* PX4_INFO */
PX4_WARN(" (↓↓ %u)", msg_start_pos);
#endif /* PX4_WARN */
// All we've checked so far is garbage, drop it - but save unchecked bytes
memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos);
@ -176,11 +176,11 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer @@ -176,11 +176,11 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
if (msg_start_pos + header_size + payload_len > rx_buff_pos) {
// If there's garbage at the beginning, drop it
if (msg_start_pos > 0) {
#ifndef PX4_INFO
printf(" (↓ %u)\n", msg_start_pos);
#ifndef PX4_WARN
printf("\033[1;33m[ micrortps_transport ]\t (↓ %u)\033[0m\n", msg_start_pos);
#else
PX4_INFO(" (↓ %u)", msg_start_pos);
#endif /* PX4_INFO */
PX4_WARN(" (↓ %u)", msg_start_pos);
#endif /* PX4_WARN */
memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos);
rx_buff_pos -= msg_start_pos;
}
@ -193,11 +193,9 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer @@ -193,11 +193,9 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
if (read_crc != calc_crc) {
#ifndef PX4_ERR
printf("Bad CRC %u != %u\n", read_crc, calc_crc);
printf(" (↓ %lu)\n", (unsigned long)(header_size + payload_len));
printf("\033[0;31m[ micrortps_transport ]\tBad CRC %u != %u\t\t(↓ %lu)\033[0m\n", read_crc, calc_crc, (unsigned long)(header_size + payload_len));
#else
PX4_ERR("Bad CRC %u != %u", read_crc, calc_crc);
PX4_ERR(" (↓ %lu)", (unsigned long)(header_size + payload_len));
PX4_ERR("Bad CRC %u != %u\t\t(↓ %lu)", read_crc, calc_crc, (unsigned long)(header_size + payload_len));
#endif /* PX4_ERR */
len = -1;
@ -249,10 +247,12 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng @@ -249,10 +247,12 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng
return len + sizeof(header);
}
UART_node::UART_node(const char *_uart_name, uint32_t _baudrate, uint32_t _poll_ms):
UART_node::UART_node(const char *_uart_name, const uint32_t _baudrate, const uint32_t _poll_ms, const bool _hw_flow_control, const bool _sw_flow_control):
uart_fd(-1),
baudrate(_baudrate),
poll_ms(_poll_ms)
poll_ms(_poll_ms),
hw_flow_control(_hw_flow_control),
sw_flow_control(_sw_flow_control)
{
if (nullptr != _uart_name) {
@ -272,9 +272,9 @@ int UART_node::init() @@ -272,9 +272,9 @@ int UART_node::init()
if (uart_fd < 0) {
#ifndef PX4_ERR
printf("Failed to open device: %s (%d)\n", uart_name, errno);
printf("\033[0;31m[ micrortps_transport ]\tUART transport: Failed to open device: %s (%d)\033[0m\n", uart_name, errno);
#else
PX4_ERR("Failed to open device: %s (%d)", uart_name, errno);
PX4_ERR("UART transport: Failed to open device: %s (%d)", uart_name, errno);
#endif /* PX4_ERR */
return -errno;
}
@ -292,61 +292,67 @@ int UART_node::init() @@ -292,61 +292,67 @@ int UART_node::init()
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
int errno_bkp = errno;
#ifndef PX4_ERR
printf("ERR GET CONF %s: %d (%d)\n", uart_name, termios_state, errno);
printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR GET CONF %s: %d (%d)\n\033[0m", uart_name, termios_state, errno);
#else
PX4_ERR("ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno);
PX4_ERR("UART transport: ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno);
#endif /* PX4_ERR */
close();
return -errno_bkp;
}
//Set up the UART for non-canonical binary communication: 8 bits, 1 stop bit, no parity,
//no flow control, no modem control
uart_config.c_iflag &= !(INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXANY | IXOFF);
uart_config.c_iflag |= IGNBRK | IGNPAR;
uart_config.c_oflag &= !(OPOST | ONLCR | OCRNL | ONOCR | ONLRET | OFILL | NLDLY | VTDLY);
uart_config.c_oflag |= NL0 | VT0;
// Set up the UART for non-canonical binary communication: 8 bits, 1 stop bit, no parity.
uart_config.c_iflag &= !(INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXANY | IXOFF);
uart_config.c_iflag |= IGNBRK | IGNPAR;
uart_config.c_oflag &= !(OPOST | ONLCR | OCRNL | ONOCR | ONLRET | OFILL | NLDLY | VTDLY);
uart_config.c_oflag |= NL0 | VT0;
uart_config.c_cflag &= !(CSIZE | CSTOPB | PARENB);
uart_config.c_cflag |= CS8 | CREAD | CLOCAL;
uart_config.c_cflag &= !(CSIZE | CSTOPB | PARENB);
uart_config.c_cflag |= CS8 | CREAD | CLOCAL;
uart_config.c_lflag &= !(ISIG | ICANON | ECHO | TOSTOP | IEXTEN);
uart_config.c_lflag &= !(ISIG | ICANON | ECHO | TOSTOP | IEXTEN);
// USB serial is indicated by /dev/ttyACM0
if (strcmp(uart_name, "/dev/ttyACM0") != 0 && strcmp(uart_name, "/dev/ttyACM1") != 0) {
// Set baud rate
speed_t speed;
// Flow control
if (hw_flow_control) {
// HW flow control
uart_config.c_lflag |= CRTSCTS;
} else if (sw_flow_control) {
// SW flow control
uart_config.c_lflag |= (IXON | IXOFF | IXANY);
}
// Set baud rate
speed_t speed;
if (!baudrate_to_speed(baudrate, &speed)) {
if (!baudrate_to_speed(baudrate, &speed)) {
#ifndef PX4_ERR
printf("ERR SET BAUD %s: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n",
uart_name, baudrate);
printf("\033[0;31mUART transport: ERR SET BAUD %s: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\033[0m\n",
uart_name, baudrate);
#else
PX4_ERR("ERR SET BAUD %s: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n",
uart_name, baudrate);
PX4_ERR("UART transport: ERR SET BAUD %s: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n",
uart_name, baudrate);
#endif /* PX4_ERR */
close();
return -EINVAL;
}
close();
return -EINVAL;
}
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
int errno_bkp = errno;
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
int errno_bkp = errno;
#ifndef PX4_ERR
printf("ERR SET BAUD %s: %d (%d)\n", uart_name, termios_state, errno);
printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: %d (%d)\033[0m\n", uart_name, termios_state, errno);
#else
PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno);
PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno);
#endif /* PX4_ERR */
close();
return -errno_bkp;
}
close();
return -errno_bkp;
}
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
int errno_bkp = errno;
#ifndef PX4_ERR
printf("ERR SET CONF %s (%d)\n", uart_name, errno);
printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET CONF %s (%d)\033[0m\n", uart_name, errno);
#else
PX4_ERR("ERR SET CONF %s (%d)", uart_name, errno);
PX4_ERR("UART transport: ERR SET CONF %s (%d)", uart_name, errno);
#endif /* PX4_ERR */
close();
return -errno_bkp;
@ -370,15 +376,15 @@ int UART_node::init() @@ -370,15 +376,15 @@ int UART_node::init()
if (flush) {
#ifndef PX4_INFO
printf("Flush\n");
printf("[ micrortps_transport ]\tUART transport: Flush\n");
#else
PX4_INFO("Flush");
PX4_INFO("UART transport: Flush");
#endif /* PX4_INFO */
} else {
#ifndef PX4_INFO
printf("No flush\n");
printf("[ micrortps_transport ]\tUART transport: No flush\n");
#else
PX4_INFO("No flush");
PX4_INFO("UART transport: No flush");
#endif /* PX4_INFO */
}
@ -397,9 +403,9 @@ uint8_t UART_node::close() @@ -397,9 +403,9 @@ uint8_t UART_node::close()
{
if (-1 != uart_fd) {
#ifndef PX4_WARN
printf("Closed UART...\n");
printf("\033[1;33m[ micrortps_transport ]\tClosed UART.\n\033[0m");
#else
PX4_WARN("Closed UART...");
PX4_WARN("Closed UART.");
#endif /* PX4_WARN */
::close(uart_fd);
uart_fd = -1;
@ -557,30 +563,30 @@ int UDP_node::init_receiver(uint16_t udp_port) @@ -557,30 +563,30 @@ int UDP_node::init_receiver(uint16_t udp_port)
if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
#ifndef PX4_ERR
printf("Create socket failed\n");
printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n");
#else
PX4_ERR("Create socket failed");
PX4_ERR("UDP transport: Create socket failed");
#endif /* PX4_ERR */
return -1;
}
#ifndef PX4_INFO
printf("- Trying to connect...");
printf("[ micrortps_transport ]\tUDP transport: Trying to connect...");
#else
PX4_INFO("Trying to connect...");
PX4_INFO("UDP transport: Trying to connect...");
#endif /* PX4_INFO */
if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) {
#ifndef PX4_ERR
printf("Bind failed\n");
printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Bind failed\033[0m\n");
#else
PX4_ERR("Bind failed");
PX4_ERR("UDP transport: Bind failed");
#endif /* PX4_ERR */
return -1;
}
#ifndef PX4_INFO
printf("Connected to server!\n\n");
printf("[ micrortps_transport ]\tUDP transport: Connected to server!\n\n");
#else
PX4_INFO("Connected to server!");
PX4_INFO("UDP transport: Connected to server!");
#endif /* PX4_INFO */
#endif /* __PX4_NUTTX */
return 0;
@ -592,9 +598,9 @@ int UDP_node::init_sender(uint16_t udp_port) @@ -592,9 +598,9 @@ int UDP_node::init_sender(uint16_t udp_port)
if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
#ifndef PX4_ERR
printf("Create socket failed\n");
printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n");
#else
PX4_ERR("Create socket failed");
PX4_ERR("UDP transport: Create socket failed");
#endif /* PX4_ERR */
return -1;
}
@ -605,9 +611,9 @@ int UDP_node::init_sender(uint16_t udp_port) @@ -605,9 +611,9 @@ int UDP_node::init_sender(uint16_t udp_port)
if (inet_aton(udp_ip, &sender_outaddr.sin_addr) == 0) {
#ifndef PX4_ERR
printf("inet_aton() failed\n");
printf("\033[0;31m[ micrortps_transport ]\tUDP transport: inet_aton() failed\033[0m\n");
#else
PX4_ERR("inet_aton() failed");
PX4_ERR("UDP transport: inet_aton() failed");
#endif /* PX4_ERR */
return -1;
}
@ -623,9 +629,9 @@ uint8_t UDP_node::close() @@ -623,9 +629,9 @@ uint8_t UDP_node::close()
if (sender_fd != -1) {
#ifndef PX4_WARN
printf("Closed sender socket!\n");
printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed sender socket!\033[0m\n");
#else
PX4_WARN("Closed sender socket!");
PX4_WARN("UDP transport: Closed sender socket!");
#endif /* PX4_WARN */
shutdown(sender_fd, SHUT_RDWR);
::close(sender_fd);
@ -634,9 +640,9 @@ uint8_t UDP_node::close() @@ -634,9 +640,9 @@ uint8_t UDP_node::close()
if (receiver_fd != -1) {
#ifndef PX4_WARN
printf("Closed receiver socket!\n");
printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed receiver socket!\033[0m\n");
#else
PX4_WARN("Closed receiver socket!");
PX4_WARN("UDP transport: Closed receiver socket!");
#endif /* PX4_WARN */
shutdown(receiver_fd, SHUT_RDWR);
::close(receiver_fd);

4
msg/templates/urtps/microRTPS_transport.h
Unescape View File

@ -94,7 +94,7 @@ private: @@ -94,7 +94,7 @@ private:
class UART_node: public Transport_node
{
public:
UART_node(const char *uart_name, uint32_t baudrate, uint32_t poll_ms);
UART_node(const char *_uart_name, const uint32_t _baudrate, const uint32_t _poll_ms, const bool _hw_flow_control, const bool _sw_flow_control);
virtual ~UART_node();
int init();
@ -110,6 +110,8 @@ protected: @@ -110,6 +110,8 @@ protected:
char uart_name[64] = {};
uint32_t baudrate;
uint32_t poll_ms;
bool hw_flow_control = false;
bool sw_flow_control = false;
struct pollfd poll_fd[1] = {};
};

2
src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
Unescape View File

@ -80,6 +80,8 @@ struct options { @@ -80,6 +80,8 @@ struct options {
uint32_t baudrate = BAUDRATE;
uint32_t poll_ms = POLL_MS;
int loops = LOOPS;
bool sw_flow_control = false;
bool hw_flow_control = false;
};
extern struct options _options;

31
src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
Unescape View File

@ -70,6 +70,8 @@ static void usage(const char *name) @@ -70,6 +70,8 @@ static void usage(const char *name)
PRINT_MODULE_USAGE_PARAM_INT('r', 2019, 0, 65536, "Select UDP Network Port for receiving (local)", true);
PRINT_MODULE_USAGE_PARAM_INT('s', 2020, 0, 65536, "Select UDP Network Port for sending (remote)", true);
PRINT_MODULE_USAGE_PARAM_STRING('i', "127.0.0.1", "<x.x.x.x>", "Select IP address (remote)", true);
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Activate UART link SW flow control", true);
PRINT_MODULE_USAGE_PARAM_FLAG('h', "Activate UART link HW flow control", true);
PRINT_MODULE_USAGE_COMMAND("stop");
PRINT_MODULE_USAGE_COMMAND("status");
@ -81,7 +83,7 @@ static int parse_options(int argc, char *argv[]) @@ -81,7 +83,7 @@ static int parse_options(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "t:d:l:w:b:p:r:s:i:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "t:d:l:w:b:p:r:s:i:fh", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ?
options::eTransports::UDP
@ -89,18 +91,22 @@ static int parse_options(int argc, char *argv[]) @@ -89,18 +91,22 @@ static int parse_options(int argc, char *argv[])
case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break;
case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break;
case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break;
case 'w': _options.sleep_ms = strtoul(myoptarg, nullptr, 10); break;
case 'w': _options.sleep_ms = strtoul(myoptarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(myoptarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(myoptarg, nullptr, 10); break;
case 'r': _options.recv_port = strtoul(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 's': _options.send_port = strtoul(myoptarg, nullptr, 10); break;
case 'i': if (nullptr != myoptarg) strcpy(_options.ip, myoptarg); break;
case 'f': _options.sw_flow_control = true; break;
case 'h': _options.hw_flow_control = true; break;
default:
usage(argv[1]);
return -1;
@ -112,6 +118,11 @@ static int parse_options(int argc, char *argv[]) @@ -112,6 +118,11 @@ static int parse_options(int argc, char *argv[])
PX4_ERR("poll timeout too low, using 1 ms");
}
if (_options.hw_flow_control && _options.sw_flow_control) {
PX4_ERR("HW and SW flow control set. Please set only one or another");
return -1;
}
return 0;
}
@ -125,9 +136,11 @@ static int micrortps_start(int argc, char *argv[]) @@ -125,9 +136,11 @@ static int micrortps_start(int argc, char *argv[])
switch (_options.transport) {
case options::eTransports::UART: {
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
PX4_INFO("UART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms",
_options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms);
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms,
_options.sw_flow_control, _options.hw_flow_control);
PX4_INFO("UART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms; flow_control: %s",
_options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms,
_options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No"));
}
break;

Write Preview
Loading…
Cancel
Save