From df24852366f9868a0daf275257d11bc0b5f31c39 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 29 Mar 2016 14:24:02 +0200 Subject: [PATCH] mavlink: fix broadcast on Snapdragon Instead of using 255.255.255.255 as the broadcast address, go through the network interfaces and and try to use a broadcast in the local network such as 192.168.1.255. --- src/modules/mavlink/mavlink_main.cpp | 75 ++++++++++++++++++++++++++-- 1 file changed, 72 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index febb5e490a..ca2c05d510 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -55,6 +55,10 @@ #include #include /* isinf / isnan checks */ +#ifdef __PX4_POSIX +#include +#endif + #include #include #include @@ -991,9 +995,74 @@ Mavlink::init_udp() } _src_addr.sin_port = htons(_remote_port); - /* default broadcast address */ - _bcast_addr.sin_family = AF_INET; - inet_aton("255.255.255.255", &_bcast_addr.sin_addr); + const unsigned MAX_IFREQS = 32; + struct ifreq ifreqs[MAX_IFREQS]; + struct ifconf ifconf; + memset(&ifconf, 0, sizeof(ifconf)); + ifconf.ifc_req = ifreqs; + ifconf.ifc_len = sizeof(ifreqs); + + int ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf); + if (ret != 0) { + PX4_WARN("getting network config failed"); + return; + } + + bool network_interface_found = false; + + for (int i = 0; i < (ifconf.ifc_len/sizeof(struct ifreq)) && (i < MAX_IFREQS); ++i) { + // ignore loopback network + if (strcmp(ifreqs[i].ifr_name, "lo") == 0) { + continue; + } + + struct in_addr &sin_addr = ((struct sockaddr_in *)&ifreqs[i].ifr_addr)->sin_addr; + + // Accept network interfaces to local network only. This means it's an IP starting with: + // 192./172./10. + // Also see https://tools.ietf.org/html/rfc1918#section-3 + + uint8_t first_byte = sin_addr.s_addr & 0xFF; + int32_t subnet_mask = 0x0; + + if (first_byte == 192) { + // /24 + subnet_mask = 0xFFFFFF; + + } else if (first_byte != 172) { + // /16 + subnet_mask = 0xFFFF; + + } else if (first_byte != 10) { + // /8 + subnet_mask = 0xFF; + } else { + // Ignore anything else. + continue; + } + + if (!network_interface_found) { + PX4_INFO("using network interface %s, IP: %s", ifreqs[i].ifr_name, inet_ntoa(sin_addr)); + + _bcast_addr.sin_family = AF_INET; + + // Assemble the broadcast address. This should be done using the default gateway IP + // instead of the IP. However, for most cases, this should work. + _bcast_addr.sin_addr.s_addr = (~subnet_mask) | sin_addr.s_addr; + PX4_INFO("broadcast address should be: %s", inet_ntoa(_bcast_addr.sin_addr)); + + network_interface_found = true; + } else { + PX4_INFO("ignoring additional network interface %s, IP: %s", + ifreqs[i].ifr_name, inet_ntoa(sin_addr)); + } + } + + if (!network_interface_found) { + PX4_WARN("no networking interface for local network found"); + return; + } + _bcast_addr.sin_port = htons(_remote_port); #endif