diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp index eae8e2a185..deaff5ef3f 100644 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -36,10 +36,12 @@ #include #include #include +#if __has_include("px4_log.h") && __has_include("px4_time.h") #include #include +#endif -#include +#include "microRTPS_transport.h" #define DEFAULT_UART "/dev/ttyACM0" @@ -117,7 +119,11 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer int errsv = errno; if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) { +#ifndef PX4_ERR + printf("Read fail %d\n", errsv); +#else PX4_ERR("Read fail %d", errsv); +#endif /* PX4_ERR */ } return len; @@ -143,7 +149,12 @@ 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)", msg_start_pos); +#else PX4_INFO(" (↓↓ %u)", msg_start_pos); +#endif /* PX4_INFO */ + // 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; @@ -166,7 +177,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)", msg_start_pos); +#else PX4_INFO(" (↓ %u)", msg_start_pos); +#endif /* PX4_INFO */ memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); rx_buff_pos -= msg_start_pos; } @@ -178,8 +193,13 @@ 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) { +#ifndef PX4_ERR + printf("Bad CRC %u != %u", read_crc, calc_crc); + printf(" (↓ %lu)", (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)); +#endif /* PX4_ERR */ len = -1; } else { @@ -270,7 +290,11 @@ int UART_node::init() uart_fd = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); if (uart_fd < 0) { +#ifndef PX4_ERR + printf("Failed to open device: %s (%d)", uart_name, errno); +#else PX4_ERR("Failed to open device: %s (%d)", uart_name, errno); +#endif /* PX4_ERR */ return -errno; } @@ -286,11 +310,14 @@ 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; +#ifndef PX4_ERR + printf("ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno); +#else PX4_ERR("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); @@ -310,15 +337,24 @@ int UART_node::init() 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); +#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); + uart_name, baudrate); +#endif /* PX4_ERR */ close(); return -EINVAL; } 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)", uart_name, termios_state, errno); +#else PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno); +#endif /* PX4_ERR */ close(); return -errno_bkp; } @@ -326,7 +362,11 @@ int UART_node::init() if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { int errno_bkp = errno; +#ifndef PX4_ERR + printf("ERR SET CONF %s (%d)", uart_name, errno); +#else PX4_ERR("ERR SET CONF %s (%d)", uart_name, errno); +#endif /* PX4_ERR */ close(); return -errno_bkp; } @@ -335,16 +375,30 @@ int UART_node::init() bool flush = false; while (0 < ::read(uart_fd, (void *)&aux, 64)) { - //PX4_INFO("%s ", aux); flush = true; +/** + * According to px4_time.h, px4_usleep() is only defined when lockstep is set + * to be used + */ +#ifndef ENABLE_LOCKSTEP_SCHEDULER + usleep(1000); +#else px4_usleep(1000); +#endif /* ENABLE_LOCKSTEP_SCHEDULER */ } if (flush) { +#ifndef PX4_INFO + printf("Flush"); +#else PX4_INFO("Flush"); - +#endif /* PX4_INFO */ } else { +#ifndef PX4_INFO + printf("No flush"); +#else PX4_INFO("No flush"); +#endif /* PX4_INFO */ } poll_fd[0].fd = uart_fd; @@ -361,7 +415,11 @@ bool UART_node::fds_OK() uint8_t UART_node::close() { if (-1 != uart_fd) { +#ifndef PX4_WARN + printf("Closed UART..."); +#else PX4_WARN("Closed UART..."); +#endif /* PX4_WARN */ ::close(uart_fd); uart_fd = -1; memset(&poll_fd, 0, sizeof(poll_fd)); @@ -514,20 +572,33 @@ 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) { +#ifndef PX4_ERR + printf("Create socket failed"); +#else PX4_ERR("Create socket failed"); +#endif /* PX4_ERR */ return -1; } - +#ifndef PX4_INFO + printf("Trying to connect..."); +#else PX4_INFO("Trying to connect..."); +#endif /* PX4_INFO */ if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) { +#ifndef PX4_ERR + printf("Bind failed"); +#else PX4_ERR("Bind failed"); +#endif /* PX4_ERR */ return -1; } - +#ifndef PX4_INFO + printf("Connected to server!"); +#else PX4_INFO("Connected to server!"); +#endif /* PX4_INFO */ #endif /* __PX4_NUTTX */ - return 0; } @@ -536,7 +607,11 @@ int UDP_node::init_sender(uint16_t udp_port) #ifndef __PX4_NUTTX if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { +#ifndef PX4_ERR + printf("Create socket failed"); +#else PX4_ERR("Create socket failed"); +#endif /* PX4_ERR */ return -1; } @@ -545,7 +620,11 @@ 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) { +#ifndef PX4_ERR + printf("inet_aton() failed"); +#else PX4_ERR("inet_aton() failed"); +#endif /* PX4_ERR */ return -1; } @@ -559,14 +638,22 @@ uint8_t UDP_node::close() #ifndef __PX4_NUTTX if (sender_fd != -1) { +#ifndef PX4_WARN + printf("Closed sender socket!"); +#else PX4_WARN("Closed sender socket!"); +#endif /* PX4_WARN */ shutdown(sender_fd, SHUT_RDWR); ::close(sender_fd); sender_fd = -1; } if (receiver_fd != -1) { +#ifndef PX4_WARN + printf("Closed receiver socket!"); +#else PX4_WARN("Closed receiver socket!"); +#endif /* PX4_WARN */ shutdown(receiver_fd, SHUT_RDWR); ::close(receiver_fd); receiver_fd = -1;