diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5edba19ed8..e8120da53c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -56,10 +56,6 @@ #include #include -#ifdef __PX4_POSIX -#include -#endif - #include #include #include @@ -1121,7 +1117,6 @@ Mavlink::find_broadcast_address() offset += sizeof(struct ifreq) #endif ) { - // Point to next network interface in buffer. cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]); @@ -1136,19 +1131,6 @@ Mavlink::find_broadcast_address() continue; } - struct ifreq bc_ifreq; - - memset(&bc_ifreq, 0, sizeof(bc_ifreq)); - - strncpy(bc_ifreq.ifr_name, cur_ifreq->ifr_name, IF_NAMESIZE); - - ret = ioctl(_socket_fd, SIOCGIFBRDADDR, &bc_ifreq); - - if (ret != 0) { - PX4_DEBUG("getting broadcast address failed for %s", cur_ifreq->ifr_name); - continue; - } - struct in_addr &sin_addr = ((struct sockaddr_in *)&cur_ifreq->ifr_addr)->sin_addr; // Accept network interfaces to local network only. This means it's an IP starting with: @@ -1162,19 +1144,21 @@ Mavlink::find_broadcast_address() } if (!_broadcast_address_found) { - PX4_INFO("using network interface %s, IP: %s", cur_ifreq->ifr_name, inet_ntoa(sin_addr)); + const struct in_addr netmask_addr = query_netmask_addr(_socket_fd, *cur_ifreq); + const struct in_addr broadcast_addr = compute_broadcast_addr(sin_addr, netmask_addr); - struct in_addr &bc_addr = ((struct sockaddr_in *)&bc_ifreq.ifr_broadaddr)->sin_addr; - PX4_INFO("with broadcast IP: %s", inet_ntoa(bc_addr)); + PX4_INFO("using network interface %s, IP: %s", cur_ifreq->ifr_name, inet_ntoa(sin_addr)); + PX4_INFO("with netmask: %s", inet_ntoa(netmask_addr)); + PX4_INFO("and broadcast IP: %s", inet_ntoa(broadcast_addr)); _bcast_addr.sin_family = AF_INET; - _bcast_addr.sin_addr = bc_addr; + _bcast_addr.sin_addr = broadcast_addr; _broadcast_address_found = true; } else { - PX4_INFO("ignoring additional network interface %s, IP: %s", - cur_ifreq->ifr_name, inet_ntoa(sin_addr)); + PX4_DEBUG("ignoring additional network interface %s, IP: %s", + cur_ifreq->ifr_name, inet_ntoa(sin_addr)); } } @@ -1201,6 +1185,28 @@ Mavlink::find_broadcast_address() #endif } +#ifdef __PX4_POSIX +const in_addr +Mavlink::query_netmask_addr(const int socket_fd, const ifreq &ifreq) +{ + struct ifreq netmask_ifreq; + memset(&netmask_ifreq, 0, sizeof(netmask_ifreq)); + strncpy(netmask_ifreq.ifr_name, ifreq.ifr_name, IF_NAMESIZE); + ioctl(socket_fd, SIOCGIFNETMASK, &netmask_ifreq); + + return ((struct sockaddr_in *)&netmask_ifreq.ifr_addr)->sin_addr; +} + +const in_addr +Mavlink::compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask_addr) +{ + struct in_addr broadcast_addr; + broadcast_addr.s_addr = ~netmask_addr.s_addr | host_addr.s_addr; + + return broadcast_addr; +} +#endif + void Mavlink::init_udp() { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 51b6e3112a..46c501e967 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -59,6 +59,10 @@ #include #include +#ifdef __PX4_POSIX +#include +#endif + #include #include #include @@ -417,6 +421,10 @@ public: int get_socket_fd() { return _socket_fd; }; #ifdef __PX4_POSIX + const in_addr query_netmask_addr(const int socket_fd, const ifreq &ifreq); + + const in_addr compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask_addr); + struct sockaddr_in *get_client_source_address() { return &_src_addr; } void set_client_source_initialized() { _src_addr_initialized = true; }