diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.template b/msg/templates/uorb_microcdr/microRTPS_client.cpp.template index 762ae88aa9..92cb6690c7 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.template +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.template @@ -130,7 +130,7 @@ void* send(void* /*unused*/) struct timespec end; px4_clock_gettime(CLOCK_REALTIME, &end); double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000); - printf("\nSENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s\n", + PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s", sent, loop, total_sent, elapsed_secs, (double)total_sent/(1000*elapsed_secs)); return nullptr; @@ -203,7 +203,7 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r break; @[end for]@ default: - printf("Unexpected topic ID\n"); + PX4_WARN("Unexpected topic ID\n"); break; } } diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp index b0ba11117c..0b89cbba5e 100644 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -116,7 +117,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer int errsv = errno; if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) { - printf("Read fail %d\n", errsv); + PX4_ERR("Read fail %d", errsv); } return len; @@ -142,7 +143,7 @@ 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) { - printf(" (↓↓ %u)\n", msg_start_pos); + PX4_INFO(" (↓↓ %u)", msg_start_pos); // 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); rx_buff_pos = rx_buff_pos - msg_start_pos; @@ -165,7 +166,7 @@ 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) { - printf(" (↓ %u)\n", msg_start_pos); + PX4_INFO(" (↓ %u)", msg_start_pos); memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); rx_buff_pos -= msg_start_pos; } @@ -177,8 +178,8 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer uint16_t calc_crc = crc16((uint8_t *)rx_buffer + msg_start_pos + header_size, payload_len); if (read_crc != calc_crc) { - printf("BAD CRC %u != %u\n", read_crc, calc_crc); - printf(" (↓ %lu)\n", (unsigned long)(header_size + payload_len)); + PX4_ERR("Bad CRC %u != %u", read_crc, calc_crc); + PX4_ERR(" (↓ %lu)", (unsigned long)(header_size + payload_len)); len = -1; } else { @@ -241,8 +242,8 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng err: //int errsv = errno; - //if (len == -1 ) printf(" => Writing error '%d'\n", errsv); - //else printf(" => Wrote '%ld' != length(%lu) error '%d'\n", (long)len, (unsigned long)length, errsv); + //if (len == -1 ) PX4_ERR(" => Writing error '%d'", errsv); + //else PX4_ERR(" => Wrote '%ld' != length(%lu) error '%d'", (long)len, (unsigned long)length, errsv); return len; } @@ -269,7 +270,7 @@ int UART_node::init() uart_fd = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); if (uart_fd < 0) { - printf("failed to open device: %s (%d)\n", uart_name, errno); + PX4_ERR("Failed to open device: %s (%d)", uart_name, errno); return -errno; } @@ -285,7 +286,7 @@ int UART_node::init() // Back up the original uart configuration to restore it after exit if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { int errno_bkp = errno; - printf("ERR GET CONF %s: %d (%d)\n", uart_name, termios_state, errno); + PX4_ERR("ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno); close(); return -errno_bkp; } @@ -308,7 +309,7 @@ int UART_node::init() // Set baud rate if (cfsetispeed(&uart_config, baudrate) < 0 || cfsetospeed(&uart_config, baudrate) < 0) { int errno_bkp = errno; - printf("ERR SET BAUD %s: %d (%d)\n", uart_name, termios_state, errno); + PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno); close(); return -errno_bkp; } @@ -316,7 +317,7 @@ int UART_node::init() if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { int errno_bkp = errno; - printf("ERR SET CONF %s (%d)\n", uart_name, errno); + PX4_ERR("ERR SET CONF %s (%d)", uart_name, errno); close(); return -errno_bkp; } @@ -325,16 +326,16 @@ int UART_node::init() bool flush = false; while (0 < ::read(uart_fd, (void *)&aux, 64)) { - //printf("%s ", aux); + //PX4_INFO("%s ", aux); flush = true; px4_usleep(1000); } if (flush) { - printf("flush\n"); + PX4_INFO("Flush"); } else { - printf("no flush\n"); + PX4_INFO("No flush"); } poll_fd[0].fd = uart_fd; @@ -351,7 +352,7 @@ bool UART_node::fds_OK() uint8_t UART_node::close() { if (-1 != uart_fd) { - printf("Close UART\n"); + PX4_WARN("Closed UART..."); ::close(uart_fd); uart_fd = -1; memset(&poll_fd, 0, sizeof(poll_fd)); @@ -423,18 +424,18 @@ int UDP_node::init_receiver(uint16_t udp_port) receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY); if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - printf("create socket failed\n"); + PX4_ERR("Create socket failed"); return -1; } - printf("Trying to connect...\n"); + PX4_INFO("Trying to connect..."); if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) { - printf("bind failed\n"); + PX4_ERR("Bind failed"); return -1; } - printf("connected to server!\n"); + PX4_INFO("Connected to server!"); #endif /* __PX4_NUTTX */ return 0; @@ -445,7 +446,7 @@ int UDP_node::init_sender(uint16_t udp_port) #ifndef __PX4_NUTTX if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - printf("create socket failed\n"); + PX4_ERR("Create socket failed"); return -1; } @@ -454,7 +455,7 @@ int UDP_node::init_sender(uint16_t udp_port) sender_outaddr.sin_port = htons(udp_port); if (inet_aton("127.0.0.1", &sender_outaddr.sin_addr) == 0) { - printf("inet_aton() failed\n"); + PX4_ERR("inet_aton() failed"); return -1; } @@ -468,14 +469,14 @@ uint8_t UDP_node::close() #ifndef __PX4_NUTTX if (sender_fd != -1) { - printf("Close sender socket\n"); + PX4_WARN("Closed sender socket!"); shutdown(sender_fd, SHUT_RDWR); ::close(sender_fd); sender_fd = -1; } if (receiver_fd != -1) { - printf("Close receiver socket\n"); + PX4_WARN("Closed receiver socket!"); shutdown(receiver_fd, SHUT_RDWR); ::close(receiver_fd); receiver_fd = -1; 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 7813346aba..78fa8f130b 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -152,7 +152,7 @@ static int parse_options(int argc, char *argv[]) static int micrortps_start(int argc, char *argv[]) { if (0 > parse_options(argc, argv)) { - printf("EXITING...\n"); + PX4_INFO("EXITING..."); _rtps_task = -1; return -1; } @@ -160,26 +160,26 @@ static int micrortps_start(int argc, char *argv[]) switch (_options.transport) { case options::eTransports::UART: { transport_node = new UART_node(_options.device, _options.baudrate.code, _options.poll_ms); - printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n", - _options.device, _options.baudrate.val, _options.sleep_ms, _options.poll_ms); + PX4_INFO("UART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms", + _options.device, _options.baudrate.val, _options.sleep_ms, _options.poll_ms); } break; case options::eTransports::UDP: { transport_node = new UDP_node(_options.recv_port, _options.send_port); - printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n", - _options.recv_port, _options.send_port, _options.sleep_ms); + PX4_INFO("UDP transport: recv port: %u; send port: %u; sleep: %dms", + _options.recv_port, _options.send_port, _options.sleep_ms); } break; default: _rtps_task = -1; - printf("EXITING...\n"); + PX4_INFO("EXITING..."); return -1; } if (0 > transport_node->init()) { - printf("EXITING...\n"); + PX4_INFO("EXITING..."); _rtps_task = -1; return -1; } @@ -199,14 +199,14 @@ static int micrortps_start(int argc, char *argv[]) double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec) / double(1000000000); - printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n", - (unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs)); + PX4_INFO("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s", + (unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs)); delete transport_node; transport_node = nullptr; - PX4_INFO("exiting"); + PX4_INFO("Stopped!"); fflush(stdout);