AP_HAL: added connect_timeout to SocketAPM

libraries/AP_HAL/utility/Socket.cpp

@@ -20,6 +20,7 @@
 #if HAL_OS_SOCKETS
 
 #include "Socket.h"
+#include <errno.h>
 
 /*
   constructor
@@ -74,6 +75,36 @@ bool SocketAPM::connect(const char *address, uint16_t port)
     return true;
 }
 
+/*
+  connect the socket with a timeout
+ */
+bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t timeout_ms)
+{
+    struct sockaddr_in sockaddr;
+    make_sockaddr(address, port, sockaddr);
+
+    set_blocking(false);
+
+    int ret = ::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
+    if (ret == 0) {
+        // instant connect?
+        return true;
+    }
+    if (errno != EINPROGRESS) {
+        return false;
+    }
+    bool pollret = pollout(timeout_ms);
+    if (!pollret) {
+        return false;
+    }
+    int sock_error = 0;
+    socklen_t len = sizeof(sock_error);
+    if (getsockopt(fd, SOL_SOCKET, SO_ERROR, (void*)&sock_error, &len) != 0) {
+        return false;
+    }
+    return sock_error == 0;
+}
+
 /*
   bind the socket
  */

libraries/AP_HAL/utility/Socket.h

@@ -36,6 +36,7 @@ public:
     ~SocketAPM();
 
     bool connect(const char *address, uint16_t port);
+    bool connect_timeout(const char *address, uint16_t port, uint32_t timeout_ms);
     bool bind(const char *address, uint16_t port);
     bool reuseaddress() const;
     bool set_blocking(bool blocking) const;