microRTPS: add debug verbose option '-v'; use while loop for reads; ease the RTTI check on timesync

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 <sending port>       UDP port for sending. Default 2020\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",
+             "  -h <hw flow control>    Activates UART link HW flow control\n"
+             "  -v <debug verbosity>    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);
         }

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
+	// ignore if rtti > 100ms
-	if (rtti > 15ll * 1000ll * 1000ll) {
+	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;
 	}

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) {
+	while (0 < (len = node_read((void *)(rx_buffer + rx_buff_pos), sizeof(rx_buffer) - rx_buff_pos))) {
-		int errsv = errno;
+		rx_buff_pos += len;
 
-		if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) {
+		// We read some
-#ifndef PX4_ERR
+		size_t header_size = sizeof(struct Header);
-			printf("\033[0;31m[ micrortps_transport ]\tRead fail %d\033[0m\n", errsv);
+
-#else
+		// but not enough
-			PX4_ERR("Read fail %d", errsv);
+		if (rx_buff_pos < header_size) {
-#endif /* PX4_ERR */
+			return 0;
 		}
 
+		uint32_t msg_start_pos = 0;
+
+		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;
+			}
+		}
+
+		// 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 */
+
+			// 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;
+		}
+
+		// [>,>,>,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;
+		}
+
+		// 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
+				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;
+			}
+
+			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_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
+			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;
+		}
+
+		// 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;
 	}
 
-	rx_buff_pos += len;
+	return 0;
-
-	// We read some
-	size_t header_size = sizeof(struct Header);
-
-	// but not enough
-	if (rx_buff_pos < header_size) {
-		return 0;
-	}
-
-	uint32_t msg_start_pos = 0;
-
-	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;
-		}
-	}
-
-	// 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);
-#else
-		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);
-		rx_buff_pos = rx_buff_pos - msg_start_pos;
-		return -1;
-	}
-
-	// [>,>,>,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;
-	}
-
-	// 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);
-#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;
-		}
-
-		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;
-
-	} 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;
-	}
-
-	// 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;
 }
 
 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
+#ifndef PX4_DEBUG
-		printf("[ micrortps_transport ]\tUART transport: Flush\n");
+		if (debug) printf("[ micrortps_transport ]\tUART transport: Flush\n");
 #else
-		PX4_INFO("UART transport: Flush");
+		if (debug) PX4_DEBUG("UART transport: Flush");
-#endif /* PX4_INFO */
+#endif /* PX4_DEBUG */
 	} else {
-#ifndef PX4_INFO
+#ifndef PX4_DEBUG
-		printf("[ micrortps_transport ]\tUART transport: No flush\n");
+		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),

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();

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;

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", "<x.x.x.x>", "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;