From 73672c90d1a6cb5b8fd677e6e280b868d7d212bc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Dec 2018 18:32:43 +1100 Subject: [PATCH] HAL_SITL: added support for multicast UDP connections this allows several copies of SITL to see each other on the same network, emulating a mesh network, which is useful for swarm testing --- libraries/AP_HAL_SITL/UARTDriver.cpp | 135 +++++++++++++++++++++++---- libraries/AP_HAL_SITL/UARTDriver.h | 10 ++ 2 files changed, 129 insertions(+), 16 deletions(-) diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index ddb2813a56..4c61e300f5 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -74,6 +74,10 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) tcp:5760:wait // tcp listen on port 5760 tcp:0:wait // tcp listen on use base_port + 0 tcpclient:192.168.2.15:5762 + udpclient:127.0.0.1 + udpclient:127.0.0.1:14550 + mcast: + mcast:239.255.145.50:14550 uart:/dev/ttyUSB0:57600 sim:ParticleSensor_SDS021: */ @@ -105,10 +109,21 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) _fd = _sitlState->sim_fd(args1, args2); } } else if (strcmp(devtype, "udpclient") == 0) { + // udp client connection const char *ip = args1; uint16_t port = args2?atoi(args2):14550; - ::printf("UDP connection %s:%u\n", ip, port); - _udp_start_client(ip, port); + if (!_connected) { + ::printf("UDP connection %s:%u\n", ip, port); + _udp_start_client(ip, port); + } + } else if (strcmp(devtype, "mcast") == 0) { + // udp multicast connection + const char *ip = args1 && *args1?args1:mcast_ip_default; + uint16_t port = args2?atoi(args2):mcast_port_default; + if (!_connected) { + ::printf("UDP multicast connection %s:%u\n", ip, port); + _udp_start_multicast(ip, port); + } } else { AP_HAL::panic("Invalid device path: %s", path); } @@ -404,6 +419,71 @@ void UARTDriver::_udp_start_client(const char *address, uint16_t port) _connected = true; } +/* + start a UDP multicast connection + */ +void UARTDriver::_udp_start_multicast(const char *address, uint16_t port) +{ + if (_connected) { + return; + } + + // establish the listening port + struct sockaddr_in sockaddr; + int ret; + + memset(&sockaddr,0,sizeof(sockaddr)); + +#ifdef HAVE_SOCK_SIN_LEN + sockaddr.sin_len = sizeof(sockaddr); +#endif + sockaddr.sin_port = htons(port); + sockaddr.sin_family = AF_INET; + sockaddr.sin_addr.s_addr = inet_addr(address); + + _mc_fd = socket(AF_INET, SOCK_DGRAM, 0); + if (_mc_fd == -1) { + fprintf(stderr, "socket failed - %s\n", strerror(errno)); + exit(1); + } + ret = fcntl(_mc_fd, F_SETFD, FD_CLOEXEC); + if (ret == -1) { + fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno)); + exit(1); + } + int one = 1; + if (setsockopt(_mc_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) { + fprintf(stderr, "setsockopt failed: %s\n", strerror(errno)); + exit(1); + } + + // close on exec, to allow reboot + fcntl(_mc_fd, F_SETFD, FD_CLOEXEC); + + ret = bind(_mc_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); + if (ret == -1) { + fprintf(stderr, "multicast bind failed on port %u - %s\n", + (unsigned)ntohs(sockaddr.sin_port), + strerror(errno)); + exit(1); + } + + struct ip_mreq mreq {}; + mreq.imr_multiaddr.s_addr = inet_addr(address); + mreq.imr_interface.s_addr = htonl(INADDR_ANY); + + ret = setsockopt(_mc_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); + if (ret == -1) { + fprintf(stderr, "multicast membership add failed on port %u - %s\n", + (unsigned)ntohs(sockaddr.sin_port), + strerror(errno)); + exit(1); + } + + // now start the outgoing connection as an ordinary UDP connection + _udp_start_client(address, port); +} + /* start a UART connection for the serial port @@ -574,7 +654,33 @@ void UARTDriver::_timer_tick(void) char buf[space]; ssize_t nread = 0; - if (!_use_send_recv) { + if (_mc_fd >= 0) { + if (_select_check(_mc_fd)) { + struct sockaddr_in from; + socklen_t fromlen = sizeof(from); + nread = recvfrom(_mc_fd, buf, space, MSG_DONTWAIT, (struct sockaddr *)&from, &fromlen); + uint16_t port = ntohs(from.sin_port); + if (_mc_myport == 0) { + // get our own address, so we can recognise packets from ourself + struct sockaddr_in myaddr; + socklen_t myaddrlen; + if (getsockname(_fd, (struct sockaddr *)&myaddr, &myaddrlen) == 0) { + _mc_myport = ntohs(myaddr.sin_port); + } + } + if (_mc_myport == port) { + // assume this is a packet from ourselves. This is not + // entirely accurate, as it could be a packet from + // another machine that has assigned the same port, + // unfortunately we don't have a better way to detect + // packets from ourselves + nread = 0; + } + } + } else if (!_use_send_recv) { + if (!_select_check(_fd)) { + return; + } int fd = _console?0:_fd; nread = ::read(fd, buf, space); if (nread == -1 && errno != EAGAIN && _uart_path) { @@ -582,19 +688,16 @@ void UARTDriver::_timer_tick(void) _fd = -1; _connected = false; } - } else { - if (_select_check(_fd)) { - nread = recv(_fd, buf, space, MSG_DONTWAIT); - if (nread <= 0 && !_is_udp) { - // the socket has reached EOF - close(_fd); - _connected = false; - fprintf(stdout, "Closed connection on serial port %u\n", _portNumber); - fflush(stdout); - return; - } - } else { - nread = 0; + } else if (_select_check(_fd)) { + nread = recv(_fd, buf, space, MSG_DONTWAIT); + if (nread <= 0 && !_is_udp) { + // the socket has reached EOF + close(_fd); + _fd = -1; + _connected = false; + fprintf(stdout, "Closed connection on serial port %u\n", _portNumber); + fflush(stdout); + return; } } if (nread > 0) { diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index e83024045d..808942c6fb 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -18,6 +18,7 @@ public: _sitlState = sitlState; _fd = -1; + _mc_fd = -1; _listen_fd = -1; } @@ -57,6 +58,9 @@ public: // file descriptor, exposed so SITL_State::loop_hook() can use it int _fd; + // file descriptor for reading multicast packets + int _mc_fd; + bool _unbuffered_writes; enum flow_control get_flow_control(void) override { return FLOW_CONTROL_ENABLE; } @@ -93,6 +97,10 @@ private: ByteBuffer _readbuffer{16384}; ByteBuffer _writebuffer{16384}; + // default multicast IP and port + const char *mcast_ip_default = "239.255.145.50"; + const uint16_t mcast_port_default = 14550; + const char *_uart_path; uint32_t _uart_baudrate; @@ -104,6 +112,7 @@ private: void _check_reconnect(); void _tcp_start_client(const char *address, uint16_t port); void _udp_start_client(const char *address, uint16_t port); + void _udp_start_multicast(const char *address, uint16_t port); void _check_connection(void); static bool _select_check(int ); static void _set_nonblocking(int ); @@ -113,6 +122,7 @@ private: uint64_t _receive_timestamp; bool _is_udp; bool _packetise; + uint16_t _mc_myport; }; #endif