MAVLink app: Fix network code to always prefer localhost, work still with remote targets

src/modules/mavlink/mavlink_main.cpp

@@ -942,11 +942,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		struct telemetry_status_s &tstatus = get_rx_status();
 
 		/* resend heartbeat via broadcast */
-		if ((_mode != MAVLINK_MODE_ONBOARD)
-			&& (_mavlink_start_time > 0 && (hrt_elapsed_time(&_mavlink_start_time) > 4 * 1000 * 1000))
-			&& (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) ||
-			(tstatus.heartbeat_time == 0)) &&
-			msgid == MAVLINK_MSG_ID_HEARTBEAT)) {
+		if ((_mode != MAVLINK_MODE_ONBOARD) &&
+			(!get_client_source_initialized()
+			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))
+			&& (msgid == MAVLINK_MSG_ID_HEARTBEAT)) {
 
 			int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
 

src/modules/mavlink/mavlink_main.h

@@ -345,6 +345,9 @@ public:
 
 	bool			get_client_source_initialized() { return _src_addr_initialized; };
 #endif
+
+	uint64_t		get_start_time() { return _mavlink_start_time; }
+
 	static bool		boot_complete() { return _boot_complete; }
 
 	bool			is_usb_uart() { return _is_usb_uart; }

src/modules/mavlink/mavlink_receiver.cpp

@@ -1814,32 +1814,38 @@ MavlinkReceiver::receive_thread(void *arg)
 
 			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))
-					|| (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) {
-				// if we were sending to localhost before but have a new host then accept him
-// This is causing issues on Linux, so use default port for now
-// this will kill tablet testing on Linux and VMs
-#ifndef __PX4_LINUX
-				srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr;
-				srcaddr_last->sin_port = srcaddr.sin_port;
-#endif
-				_mavlink->set_client_source_initialized();
+			if (!_mavlink->get_client_source_initialized()) {
+				
+				// set the address either if localhost or if 3 seconds have passed
+				// this ensures that a GCS running on localhost can get a hold of
+				// the system within the first N seconds
+				hrt_abstime stime = _mavlink->get_start_time();
+				if ((stime != 0 && (hrt_elapsed_time(&stime) > 3 * 1000 * 1000))
+					|| (srcaddr_last->sin_addr.s_addr == htonl(localhost))) {
+					srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr;
+					srcaddr_last->sin_port = srcaddr.sin_port;
+					_mavlink->set_client_source_initialized();
+				}
 			}
 #endif
-			/* if read failed, this loop won't execute */
-			for (ssize_t i = 0; i < nread; i++) {
-				if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
-					/* handle generic messages and commands */
-					handle_message(&msg);
-
-					/* handle packet with parent object */
-					_mavlink->handle_message(&msg);
+			// only start accepting messages once we're sure who we talk to
+
+			if (_mavlink->get_client_source_initialized()) {
+				/* if read failed, this loop won't execute */
+				for (ssize_t i = 0; i < nread; i++) {
+					if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
+						/* handle generic messages and commands */
+						handle_message(&msg);
+
+						/* handle packet with parent object */
+						_mavlink->handle_message(&msg);
+					}
 				}
-			}
 
-			/* count received bytes (nread will be -1 on read error) */
-			if (nread > 0) {
-				_mavlink->count_rxbytes(nread);
+				/* count received bytes (nread will be -1 on read error) */
+				if (nread > 0) {
+					_mavlink->count_rxbytes(nread);
+				}
 			}
 		}
 	}