mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 01:12:11 +00:00
microRTPS: add possibility to set HW or SW flow control; improve verbosity aesthetics
This commit is contained in:
TSC21
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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]@
|
||||
|
||||
@@ -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 <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)
|
||||
{
|
||||
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<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;
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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] = {};
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user