diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 8a9600ca4d..39738637e0 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -402,20 +402,18 @@ void UARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection) */ void UARTDriver::_tcp_start_client(const char *address, uint16_t port) { - int one=1; - struct sockaddr_in sockaddr; - int ret; if (_connected) { return; } _use_send_recv = true; - + if (_fd != -1) { close(_fd); } + struct sockaddr_in sockaddr; memset(&sockaddr,0,sizeof(sockaddr)); #ifdef HAVE_SOCK_SIN_LEN @@ -425,22 +423,36 @@ void UARTDriver::_tcp_start_client(const char *address, uint16_t port) sockaddr.sin_family = AF_INET; sockaddr.sin_addr.s_addr = inet_addr(address); - _fd = socket(AF_INET, SOCK_STREAM, 0); - if (_fd == -1) { - fprintf(stderr, "socket failed - %s\n", strerror(errno)); - exit(1); - } - ret = fcntl(_fd, F_SETFD, FD_CLOEXEC); - if (ret == -1) { - fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno)); - exit(1); + constexpr auto one=1; + int ret; + for (int attempt = 0; attempt < 3; ++attempt) { + _fd = socket(AF_INET, SOCK_STREAM, 0); + if (_fd == -1) { + fprintf(stderr, "socket failed - %s\n", strerror(errno)); + exit(1); + } + ret = fcntl(_fd, F_SETFD, FD_CLOEXEC); + if (ret == -1) { + fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno)); + exit(1); + } + + /* we want to be able to re-use ports quickly */ + setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + + ret = connect(_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); + if (ret == 0) { + break; + } + fprintf(stderr, "connect failed on port %u - %s at %d retrying\n", + (unsigned) ntohs(sockaddr.sin_port), strerror(errno), AP_HAL::millis()); + close(_fd); + // If connection failed, wait for a bit before retrying + sleep(1); } - /* we want to be able to re-use ports quickly */ - setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); - - ret = connect(_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); if (ret == -1) { + close(_fd); fprintf(stderr, "connect failed on port %u - %s\n", (unsigned)ntohs(sockaddr.sin_port), strerror(errno)); @@ -451,6 +463,7 @@ void UARTDriver::_tcp_start_client(const char *address, uint16_t port) setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); fcntl(_fd, F_SETFD, FD_CLOEXEC); _connected = true; + fprintf(stdout, "New remote connection on serial port %u, p %u at %d\n", _portNumber, (unsigned) ntohs(sockaddr.sin_port), AP_HAL::millis()); }