diff --git a/msg/templates/urtps/Publisher.cpp.em b/msg/templates/urtps/Publisher.cpp.em index 79e40c6a7b..066f929e41 100644 --- a/msg/templates/urtps/Publisher.cpp.em +++ b/msg/templates/urtps/Publisher.cpp.em @@ -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; } } } diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index 367498862d..18a7d33128 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/msg/templates/urtps/RtpsTopics.cpp.em @@ -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 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 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) 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) 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; } diff --git a/msg/templates/urtps/Subscriber.cpp.em b/msg/templates/urtps/Subscriber.cpp.em index 5cdc6a4fe7..217d835799 100644 --- a/msg/templates/urtps/Subscriber.cpp.em +++ b/msg/templates/urtps/Subscriber.cpp.em @@ -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]@ diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em index ba59e47d00..8a75064b93 100644 --- a/msg/templates/urtps/microRTPS_agent.cpp.em +++ b/msg/templates/urtps/microRTPS_agent.cpp.em @@ -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 { 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) " -p Time in ms to poll over UART. Default 1ms\n" " -r UDP port for receiving. Default 2019\n" " -s UDP port for sending. Default 2020\n" - " -i Target IP for UDP. Default 127.0.0.1\n", + " -i Target IP for UDP. Default 127.0.0.1\n" + " -f Activates UART link SW flow control\n" + " -h Activates UART link HW flow control\n", name); } @@ -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) 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) { 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) (!running && loop > 1)) { std::chrono::duration 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; diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em index 612add5b49..4bd924b194 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ b/msg/templates/urtps/microRTPS_timesync.cpp.em @@ -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 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 // 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) { 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) { diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp index 0d6b0c241c..7a199d3904 100644 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -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 // 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 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 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 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() 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() 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_cflag &= !(CSIZE | CSTOPB | PARENB); - uart_config.c_cflag |= CS8 | CREAD | CLOCAL; + uart_config.c_oflag &= !(OPOST | ONLCR | OCRNL | ONOCR | ONLRET | OFILL | NLDLY | VTDLY); + uart_config.c_oflag |= NL0 | VT0; - uart_config.c_lflag &= !(ISIG | ICANON | ECHO | TOSTOP | IEXTEN); + uart_config.c_cflag &= !(CSIZE | CSTOPB | PARENB); + uart_config.c_cflag |= CS8 | CREAD | CLOCAL; - // 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; + uart_config.c_lflag &= !(ISIG | ICANON | ECHO | TOSTOP | IEXTEN); - if (!baudrate_to_speed(baudrate, &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)) { #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() 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() { 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) 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) 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) 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() 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() 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); diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h index f179c4c863..266d414d93 100644 --- a/msg/templates/urtps/microRTPS_transport.h +++ b/msg/templates/urtps/microRTPS_transport.h @@ -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: 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] = {}; }; diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h index 685a8a45ca..529044e60a 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -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; 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 50a8bfaabd..42a1413e22 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -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", "", "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[]) 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[]) 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[]) 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[]) 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;