forked from Archive/PX4-Autopilot
microRTPS_transport: use preprocessor declarations to setup different build contexts for client and agent code
This commit is contained in:
TSC21
Lorenz Meier
parent
f0447e0009
commit
b3435dd1f5
|
|
@ -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);
|
||||
#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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue