diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 0b64b446f3..968aa9c582 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -86,7 +86,9 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) } else if (strcmp(devtype, "uart") == 0) { uint32_t baudrate = args2? atoi(args2) : baud; ::printf("UART connection %s:%u\n", args1, baudrate); - _uart_start_connection(args1, baudrate); + _uart_path = strdup(args1); + _uart_baudrate = baudrate; + _uart_start_connection(); } else { AP_HAL::panic("Invalid device path: %s", path); } @@ -299,19 +301,22 @@ void UARTDriver::_tcp_start_client(const char *address, uint16_t port) /* start a UART connection for the serial port */ -void UARTDriver::_uart_start_connection(const char *path, uint32_t baudrate) +void UARTDriver::_uart_start_connection(void) { struct termios t {}; if (!_connected) { - ::printf("Opening %s\n", path); - _fd = ::open(path, O_RDWR | O_CLOEXEC); + _fd = ::open(_uart_path, O_RDWR | O_CLOEXEC); + if (_fd == -1) { + return; + } // use much smaller buffer sizes on real UARTs _writebuffer.set_size(1024); _readbuffer.set_size(512); + ::printf("Opened %s\n", _uart_path); } if (_fd == -1) { - AP_HAL::panic("Unable to open UART %s", path); + AP_HAL::panic("Unable to open UART %s", _uart_path); } // set non-blocking @@ -332,7 +337,7 @@ void UARTDriver::_uart_start_connection(const char *path, uint32_t baudrate) // set baudrate tcgetattr(_fd, &t); - cfsetspeed(&t, baudrate); + cfsetspeed(&t, _uart_baudrate); tcsetattr(_fd, TCSANOW, &t); _connected = true; @@ -390,9 +395,18 @@ void UARTDriver::_set_nonblocking(int fd) fcntl(fd, F_SETFL, v | O_NONBLOCK); } +void UARTDriver::_check_reconnect(void) +{ + if (!_uart_path) { + return; + } + _uart_start_connection(); +} + void UARTDriver::_timer_tick(void) { if (!_connected) { + _check_reconnect(); return; } uint32_t navail; @@ -402,6 +416,11 @@ void UARTDriver::_timer_tick(void) if (readptr && navail > 0) { if (!_use_send_recv) { nwritten = ::write(_fd, readptr, navail); + if (nwritten == -1 && errno != EAGAIN && _uart_path) { + close(_fd); + _fd = -1; + _connected = false; + } } else { nwritten = send(_fd, readptr, navail, MSG_DONTWAIT); } @@ -420,6 +439,11 @@ void UARTDriver::_timer_tick(void) if (!_use_send_recv) { int fd = _console?0:_fd; nread = ::read(fd, buf, space); + if (nread == -1 && errno != EAGAIN && _uart_path) { + close(_fd); + _fd = -1; + _connected = false; + } } else { if (_select_check(_fd)) { nread = recv(_fd, buf, space, MSG_DONTWAIT); diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index a438e23dd1..d50e6886a7 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -73,11 +73,15 @@ private: ByteBuffer _readbuffer{16384}; ByteBuffer _writebuffer{16384}; + const char *_uart_path; + uint32_t _uart_baudrate; + // IPv4 address of target for uartC const char *_tcp_client_addr; void _tcp_start_connection(uint16_t port, bool wait_for_connection); - void _uart_start_connection(const char *path, uint32_t baudrate); + void _uart_start_connection(void); + void _check_reconnect(); void _tcp_start_client(const char *address, uint16_t port); void _check_connection(void); static bool _select_check(int );