microRTPS_transport: use preprocessor declarations to setup different build contexts for client and agent code

msg/templates/urtps/microRTPS_transport.cpp

@@ -36,10 +36,12 @@
 #include <errno.h>
 #include <sys/socket.h>
 #include <cstdlib>
+#if __has_include("px4_log.h") && __has_include("px4_time.h")
 #include <px4_log.h>
 #include <px4_time.h>
+#endif
 
-#include <microRTPS_transport.h>
+#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;