mavlink socket: stream to localhost before actually receiving packets

src/modules/mavlink/mavlink_main.cpp

@@ -974,11 +974,11 @@ Mavlink::init_udp()
 		return;
 	}
 
-	unsigned char inbuf[256];
-	socklen_t addrlen = sizeof(_src_addr);
+	// set default target address
+	_src_addr.sin_family = AF_INET;
+	inet_aton("127.0.0.1", &_src_addr.sin_addr);
+	_src_addr.sin_port = htons(_network_port);
 
-	// wait for client to connect to socket
-	recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen);
 #endif
 }
 

src/modules/mavlink/mavlink_main.h

@@ -47,6 +47,7 @@
 #else
 #include <sys/socket.h>
 #include <netinet/in.h>
+#include <arpa/inet.h>
 #include <drivers/device/device.h>
 #endif
 #include <systemlib/param/param.h>
@@ -333,7 +334,9 @@ public:
 	unsigned short		get_network_port() { return _network_port; }
 
 	int 			get_socket_fd () { return _socket_fd; };
-
+#ifdef __PX4_POSIX
+	struct sockaddr_in * get_client_source_address() {return &_src_addr;};
+#endif
 	static bool		boot_complete() { return _boot_complete; }
 
 protected:

src/modules/mavlink/mavlink_receiver.cpp

@@ -1776,6 +1776,13 @@ MavlinkReceiver::receive_thread(void *arg)
 			} else {
 				// could be TCP or other protocol
 			}
+
+			struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address();
+			int localhost = (127 << 24) + 1;
+			if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) {
+				// if we were sending to localhost before but have a new host then accept him
+				memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr));
+			}
 #endif
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {