From 9f8f36aad85915e59e69e4e4958c98b496d43562 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 5 Dec 2018 16:32:49 +1100
Subject: [PATCH] HAL_SITL: added udpclient support

this allows direct UDP output from SITL
---
 libraries/AP_HAL_SITL/UARTDriver.cpp | 62 +++++++++++++++++++++++++++-
 libraries/AP_HAL_SITL/UARTDriver.h   |  2 +
 2 files changed, 63 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp
index ede23bcd42..1efb3ee564 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.cpp
+++ b/libraries/AP_HAL_SITL/UARTDriver.cpp
@@ -103,6 +103,11 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
                 _connected = true;
                 _fd = _sitlState->sim_fd(args1, args2);
             }
+        } else if (strcmp(devtype, "udpclient") == 0) {
+            const char *ip = args1;
+            uint16_t port = args2?atoi(args2):14550;
+            ::printf("UDP connection %s:%u\n", ip, port);
+            _udp_start_client(ip, port);
         } else {
             AP_HAL::panic("Invalid device path: %s", path);
         }
@@ -343,6 +348,61 @@ void UARTDriver::_tcp_start_client(const char *address, uint16_t port)
 }
 
 
+/*
+  start a UDP client connection for the serial port.
+ */
+void UARTDriver::_udp_start_client(const char *address, uint16_t port)
+{
+    struct sockaddr_in sockaddr;
+    int ret;
+
+    if (_connected) {
+        return;
+    }
+
+    _use_send_recv = true;
+    
+    if (_fd != -1) {
+        close(_fd);
+    }
+
+    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);
+
+    _fd = socket(AF_INET, SOCK_DGRAM, 0);
+    if (_fd == -1) {
+        fprintf(stderr, "socket failed - %s\n", strerror(errno));
+        exit(1);
+    }
+    ret = fcntl(_fd, F_SETFD, FD_CLOEXEC);
+    if (ret == -1) {
+        fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
+        exit(1);
+    }
+
+    // try to setup for broadcast, this may fail if insufficient privileges
+    int one = 1;
+    setsockopt(_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
+
+    ret = connect(_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
+    if (ret == -1) {
+        fprintf(stderr, "udp connect failed on port %u - %s\n",
+                (unsigned)ntohs(sockaddr.sin_port),
+                strerror(errno));
+        exit(1);
+    }
+
+    _is_udp = true;
+    _connected = true;
+}
+
+
 /*
   start a UART connection for the serial port
  */
@@ -507,7 +567,7 @@ void UARTDriver::_timer_tick(void)
     } else {
         if (_select_check(_fd)) {
             nread = recv(_fd, buf, space, MSG_DONTWAIT);
-            if (nread <= 0) {
+            if (nread <= 0 && !_is_udp) {
                 // the socket has reached EOF
                 close(_fd);
                 _connected = false;
diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h
index ae0be6ff81..4785e29833 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.h
+++ b/libraries/AP_HAL_SITL/UARTDriver.h
@@ -103,6 +103,7 @@ private:
     void _uart_start_connection(void);
     void _check_reconnect();
     void _tcp_start_client(const char *address, uint16_t port);
+    void _udp_start_client(const char *address, uint16_t port);
     void _check_connection(void);
     static bool _select_check(int );
     static void _set_nonblocking(int );
@@ -110,6 +111,7 @@ private:
 
     SITL_State *_sitlState;
     uint64_t _receive_timestamp;
+    bool _is_udp;
 };
 
 #endif