diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em index 8a75064b93..8cc5b60e3c 100644 --- a/msg/templates/urtps/microRTPS_agent.cpp.em +++ b/msg/templates/urtps/microRTPS_agent.cpp.em @@ -110,6 +110,7 @@ struct options { char ip[16] = DEFAULT_IP; bool sw_flow_control = false; bool hw_flow_control = false; + bool verbose_debug = false; } _options; static void usage(const char *name) @@ -124,7 +125,8 @@ static void usage(const char *name) " -s UDP port for sending. Default 2020\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", + " -h Activates UART link HW flow control\n" + " -v Add more verbosity\n", name); } @@ -132,7 +134,7 @@ static int parse_options(int argc, char **argv) { int ch; - while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:fh")) != EOF) + while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:fhv")) != EOF) { switch (ch) { @@ -148,6 +150,7 @@ static int parse_options(int argc, char **argv) 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; + case 'v': _options.verbose_debug = true; break; default: usage(argv[0]); return -1; @@ -234,7 +237,7 @@ int main(int argc, char** argv) case options::eTransports::UART: { transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control, _options.hw_flow_control); + _options.sw_flow_control, _options.hw_flow_control, _options.verbose_debug); 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")); @@ -242,7 +245,7 @@ int main(int argc, char** argv) break; case options::eTransports::UDP: { - transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port); + transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port, _options.verbose_debug); 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); } diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em index 4bd924b194..cc4c99b2c2 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ b/msg/templates/urtps/microRTPS_timesync.cpp.em @@ -134,8 +134,8 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t } } - // ignore if rtti > 10ms - if (rtti > 15ll * 1000ll * 1000ll) { + // ignore if rtti > 100ms + if (rtti > 100ll * 1000ll * 1000ll) { std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms\033[0m" << std::endl; return false; } diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp index 7a199d3904..539392de1c 100644 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -79,7 +79,9 @@ uint16_t const crc16_table[256] = { 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 }; -Transport_node::Transport_node(): rx_buff_pos(0) +Transport_node::Transport_node(const bool _debug): + rx_buff_pos(0), + debug(_debug) { } @@ -111,106 +113,96 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer *topic_ID = 255; - ssize_t len = node_read((void *)(rx_buffer + rx_buff_pos), sizeof(rx_buffer) - rx_buff_pos); + ssize_t len = 0; - if (len <= 0) { - int errsv = errno; + while (0 < (len = node_read((void *)(rx_buffer + rx_buff_pos), sizeof(rx_buffer) - rx_buff_pos))) { + rx_buff_pos += len; - if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tRead fail %d\033[0m\n", errsv); -#else - PX4_ERR("Read fail %d", errsv); -#endif /* PX4_ERR */ - } + // We read some + size_t header_size = sizeof(struct Header); - return len; - } + // but not enough + if (rx_buff_pos < header_size) { + return 0; + } - rx_buff_pos += len; + uint32_t msg_start_pos = 0; - // We read some - size_t header_size = sizeof(struct Header); + for (msg_start_pos = 0; msg_start_pos <= rx_buff_pos - header_size; ++msg_start_pos) { + if ('>' == rx_buffer[msg_start_pos] && memcmp(rx_buffer + msg_start_pos, ">>>", 3) == 0) { + break; + } + } - // but not enough - if (rx_buff_pos < header_size) { - return 0; - } + // Start not found + if (msg_start_pos > (rx_buff_pos - header_size)) { +#ifndef PX4_DEBUG + if (debug) printf("\033[1;33m[ micrortps_transport ]\t (↓↓ %u)\033[0m\n", msg_start_pos); +#else + if (debug) PX4_DEBUG(" (↓↓ %u)", msg_start_pos); +#endif /* PX4_DEBUG */ - uint32_t msg_start_pos = 0; + // 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; + return -1; + } - for (msg_start_pos = 0; msg_start_pos <= rx_buff_pos - header_size; ++msg_start_pos) { - if ('>' == rx_buffer[msg_start_pos] && memcmp(rx_buffer + msg_start_pos, ">>>", 3) == 0) { - break; + // [>,>,>,topic_ID,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd] + struct Header *header = (struct Header *)&rx_buffer[msg_start_pos]; + uint32_t payload_len = ((uint32_t)header->payload_len_h << 8) | header->payload_len_l; + + // The message won't fit the buffer. + if (buffer_len < header_size + payload_len) { + // drop the message and continue the readings + // @note: this is just a work around to avoid the link to be closed + memmove(rx_buffer, rx_buffer + msg_start_pos + 1, rx_buff_pos - (msg_start_pos + 1)); + rx_buff_pos = rx_buff_pos - (msg_start_pos + 1); + return -EMSGSIZE; } - } - // Start not found - if (msg_start_pos > rx_buff_pos - header_size) { -#ifndef PX4_WARN - printf("\033[1;33m[ micrortps_transport ]\t (↓↓ %u)\033[0m\n", msg_start_pos); + // We do not have a complete message yet + 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_DEBUG + if (debug) printf("\033[1;33m[ micrortps_transport ]\t (↓ %u)\033[0m\n", msg_start_pos); #else - PX4_WARN(" (↓↓ %u)", msg_start_pos); -#endif /* PX4_WARN */ + if (debug) PX4_DEBUG(" (↓ %u)", msg_start_pos); +#endif /* PX4_DEBUG */ + memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); + rx_buff_pos -= 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; - return -1; - } + return 0; + } - // [>,>,>,topic_ID,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd] - struct Header *header = (struct Header *)&rx_buffer[msg_start_pos]; - uint32_t payload_len = ((uint32_t)header->payload_len_h << 8) | header->payload_len_l; - - // The message won't fit the buffer. - if (buffer_len < header_size + payload_len) { - // drop the message an continue the readings - // @note: this is just a work around to avoid the link to be closed - memmove(rx_buffer, rx_buffer + msg_start_pos + 1, rx_buff_pos - (msg_start_pos + 1)); - rx_buff_pos = rx_buff_pos - (msg_start_pos + 1); - return -EMSGSIZE; - } + uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l; + uint16_t calc_crc = crc16((uint8_t *)rx_buffer + msg_start_pos + header_size, payload_len); - // We do not have a complete message yet - 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_WARN - printf("\033[1;33m[ micrortps_transport ]\t (↓ %u)\033[0m\n", msg_start_pos); + if (read_crc != calc_crc) { +#ifndef PX4_DEBUG + if (debug) 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_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; + if (debug) PX4_DEBUG("Bad CRC %u != %u\t\t(↓ %lu)", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); +#endif /* PX4_DEBUG */ + len = -1; + + } else { + // copy message to outbuffer and set other return values + memmove(out_buffer, rx_buffer + msg_start_pos + header_size, payload_len); + *topic_ID = header->topic_ID; + len = payload_len + header_size; } - return 0; - } - - uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l; - 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("\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\t\t(↓ %lu)", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); -#endif /* PX4_ERR */ - len = -1; + // discard message from rx_buffer + rx_buff_pos -= header_size + payload_len; + memmove(rx_buffer, rx_buffer + msg_start_pos + header_size + payload_len, rx_buff_pos); - } else { - // copy message to outbuffer and set other return values - memmove(out_buffer, rx_buffer + msg_start_pos + header_size, payload_len); - *topic_ID = header->topic_ID; - len = payload_len + header_size; + return len; } - // discard message from rx_buffer - rx_buff_pos -= header_size + payload_len; - memmove(rx_buffer, rx_buffer + msg_start_pos + header_size + payload_len, rx_buff_pos); - - return len; + return 0; } size_t Transport_node::get_header_length() @@ -247,14 +239,17 @@ 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, const uint32_t _baudrate, const uint32_t _poll_ms, const bool _hw_flow_control, const bool _sw_flow_control): +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, const bool _debug): + Transport_node(_debug), uart_fd(-1), baudrate(_baudrate), poll_ms(_poll_ms), hw_flow_control(_hw_flow_control), sw_flow_control(_sw_flow_control) - { + if (nullptr != _uart_name) { strcpy(uart_name, _uart_name); } @@ -375,16 +370,16 @@ int UART_node::init() } if (flush) { -#ifndef PX4_INFO - printf("[ micrortps_transport ]\tUART transport: Flush\n"); +#ifndef PX4_DEBUG + if (debug) printf("[ micrortps_transport ]\tUART transport: Flush\n"); #else - PX4_INFO("UART transport: Flush"); -#endif /* PX4_INFO */ + if (debug) PX4_DEBUG("UART transport: Flush"); +#endif /* PX4_DEBUG */ } else { -#ifndef PX4_INFO - printf("[ micrortps_transport ]\tUART transport: No flush\n"); +#ifndef PX4_DEBUG + if (debug) printf("[ micrortps_transport ]\tUART transport: No flush\n"); #else - PX4_INFO("UART transport: No flush"); + if (debug) PX4_DEBUG("UART transport: No flush"); #endif /* PX4_INFO */ } @@ -522,7 +517,9 @@ bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed) return true; } -UDP_node::UDP_node(const char* _udp_ip, uint16_t _udp_port_recv, uint16_t _udp_port_send): +UDP_node::UDP_node(const char* _udp_ip, uint16_t _udp_port_recv, + uint16_t _udp_port_send, const bool _debug): + Transport_node(_debug), sender_fd(-1), receiver_fd(-1), udp_port_recv(_udp_port_recv), diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h index 266d414d93..d74ddc86dd 100644 --- a/msg/templates/urtps/microRTPS_transport.h +++ b/msg/templates/urtps/microRTPS_transport.h @@ -44,7 +44,7 @@ class Transport_node { public: - Transport_node(); + Transport_node(const bool _debug); virtual ~Transport_node(); virtual int init() {return 0;} @@ -94,7 +94,9 @@ private: class UART_node: public Transport_node { public: - 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_node(const char *_uart_name, const uint32_t _baudrate, + const uint32_t _poll_ms, const bool _hw_flow_control, + const bool _sw_flow_control, const bool _debug); virtual ~UART_node(); int init(); @@ -118,7 +120,8 @@ protected: class UDP_node: public Transport_node { public: - UDP_node(const char* _udp_ip, uint16_t udp_port_recv, uint16_t udp_port_send); + UDP_node(const char* _udp_ip, uint16_t udp_port_recv, uint16_t udp_port_send, + const bool _debug); virtual ~UDP_node(); int init(); diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h index 529044e60a..aa0cce5493 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -82,6 +82,7 @@ struct options { int loops = LOOPS; bool sw_flow_control = false; bool hw_flow_control = false; + bool verbose_debug = 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 42a1413e22..8eb126fabf 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -72,6 +72,7 @@ static void usage(const char *name) 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_PARAM_FLAG('v', "Add more verbosity", true); PRINT_MODULE_USAGE_COMMAND("stop"); PRINT_MODULE_USAGE_COMMAND("status"); @@ -83,7 +84,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:fh", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "t:d:l:w:b:p:r:s:i:fhv", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ? options::eTransports::UDP @@ -107,6 +108,8 @@ static int parse_options(int argc, char *argv[]) case 'h': _options.hw_flow_control = true; break; + case 'v': _options.verbose_debug = true; break; + default: usage(argv[1]); return -1; @@ -137,7 +140,7 @@ 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, - _options.sw_flow_control, _options.hw_flow_control); + _options.sw_flow_control, _options.hw_flow_control, _options.verbose_debug); 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")); @@ -145,9 +148,11 @@ static int micrortps_start(int argc, char *argv[]) break; case options::eTransports::UDP: { - transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port); + transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port, + _options.verbose_debug); PX4_INFO("UDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dms", _options.ip, _options.recv_port, _options.send_port, _options.sleep_ms); + } break;