Allow multiple UDP clients to connect/disconnect/reconnect

Tools/AP_Periph/Web/scripts/pppgw_webui.lua

@@ -938,6 +938,9 @@ local function Client(_sock, _idx)
 
    function self.remove()
       DEBUG(string.format("%u: removing client OFFSET=%u", idx, offset))
+      if self.closed then
+         return
+      end
       sock:close()
       self.closed = true
    end

libraries/AP_HAL/utility/Socket.cpp

@@ -303,7 +303,27 @@ ssize_t SOCKET_CLASS_NAME::send(const void *buf, size_t size) const
 }
 
 /*
-  send some data
+  send some data with address as a uint32_t
+ */
+ssize_t SOCKET_CLASS_NAME::sendto(const void *buf, size_t size, uint32_t address, uint16_t port)
+{
+    if (fd == -1) {
+        return -1;
+    }
+    struct sockaddr_in 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 = htonl(address);
+
+    return CALL_PREFIX(sendto)(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
+}
+
+/*
+  send some data with address as a string
  */
 ssize_t SOCKET_CLASS_NAME::sendto(const void *buf, size_t size, const char *address, uint16_t port)
 {
@@ -327,8 +347,12 @@ ssize_t SOCKET_CLASS_NAME::recv(void *buf, size_t size, uint32_t timeout_ms)
     socklen_t len = sizeof(struct sockaddr_in);
     int fin = get_read_fd();
     ssize_t ret;
-    ret = CALL_PREFIX(recvfrom)(fin, buf, size, MSG_DONTWAIT, (sockaddr *)&last_in_addr[0], &len);
-    if (ret <= 0) {
+    uint32_t in_addr[4] = {};
+    ret = CALL_PREFIX(recvfrom)(fin, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr[0], &len);
+    if (ret > 0) {
+        // only update last_in_addr if we received data
+        memcpy(last_in_addr, in_addr, sizeof(last_in_addr));
+    } else {
         if (!datagram && connected && ret == 0) {
             // remote host has closed connection
             connected = false;
@@ -380,6 +404,24 @@ const char *SOCKET_CLASS_NAME::last_recv_address(char *ip_addr_buf, uint8_t bufl
     return ret;
 }
 
+/*
+  return the IP address and port of the last received packet
+ */
+bool SOCKET_CLASS_NAME::last_recv_address(uint32_t &ip_addr, uint16_t &port) const
+{
+    const struct sockaddr_in &sin = *(struct sockaddr_in *)&last_in_addr[0];
+    if (sin.sin_family != AF_INET) {
+        return false;
+    }
+    ip_addr = ntohl(sin.sin_addr.s_addr);
+    port = ntohs(sin.sin_port);
+    if (ip_addr == 0 ||
+        port == 0) {
+        return false;
+    }
+    return true;
+}
+
 void SOCKET_CLASS_NAME::set_broadcast(void) const
 {
     if (fd == -1) {

libraries/AP_HAL/utility/Socket.hpp

@@ -43,6 +43,7 @@ class SOCKET_CLASS_NAME {
 
     ssize_t send(const void *pkt, size_t size) const;
     ssize_t sendto(const void *buf, size_t size, const char *address, uint16_t port);
+    ssize_t sendto(const void *buf, size_t size, uint32_t address, uint16_t port);
     ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms);
 
     // return the IP address and port of the last received packet
@@ -51,6 +52,9 @@ class SOCKET_CLASS_NAME {
     // return the IP address and port of the last received packet, using caller supplied buffer
     const char *last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const;
 
+    // return the IP address and port of the last received packet
+    bool last_recv_address(uint32_t &ip_addr, uint16_t &port) const;
+
     // return true if there is pending data for input
     bool pollin(uint32_t timeout_ms);
 

libraries/AP_HAL/utility/packetise.cpp

@@ -2,12 +2,11 @@
   support for sending UDP packets on MAVLink packet boundaries.
  */
 
-#include <GCS_MAVLink/GCS.h>
-
-#if HAL_GCS_ENABLED
-
 #include "packetise.h"
 
+#if AP_MAVLINK_PACKETISE_ENABLED
+#include <GCS_MAVLink/GCS_MAVLink.h>
+
 /*
   return the number of bytes to send for a packetised connection
  */
@@ -68,4 +67,4 @@ uint16_t mavlink_packetise(ByteBuffer &writebuf, uint16_t n)
     return n;
 }
 
-#endif // HAL_GCS_ENABLED
+#endif // AP_MAVLINK_PACKETISE_ENABLED

libraries/AP_HAL/utility/packetise.h

@@ -1,4 +1,10 @@
 #pragma once
+#include "RingBuffer.h"
+#include <GCS_MAVLink/GCS_config.h>
+
+#ifndef AP_MAVLINK_PACKETISE_ENABLED
+#define AP_MAVLINK_PACKETISE_ENABLED HAL_GCS_ENABLED
+#endif
 
 /*
   return the number of bytes to send for a packetised connection

libraries/AP_Networking/AP_Networking.h

@@ -266,9 +266,11 @@ class AP_Networking
         uint32_t last_size_rx;
         bool packetise;
         bool connected;
+        uint32_t last_udp_connect_address;
+        uint16_t last_udp_connect_port;
         bool have_received;
         bool close_on_recv_error;
-
+        uint32_t last_udp_srv_recv_time_ms;
         HAL_Semaphore sem;
     };
 #endif // AP_NETWORKING_REGISTER_PORT_ENABLED

libraries/AP_Networking/AP_Networking_port.cpp

@@ -255,7 +255,7 @@ void AP_Networking::Port::tcp_server_loop(void)
             sock = listen_sock->accept(100);
             if (sock != nullptr) {
                 sock->set_blocking(false);
-                char buf[16];
+                char buf[IP4_STR_LEN];
                 uint16_t last_port;
                 const char *last_addr = listen_sock->last_recv_address(buf, sizeof(buf), last_port);
                 if (last_addr != nullptr) {
@@ -345,6 +345,30 @@ bool AP_Networking::Port::send_receive(void)
         }
     }
 
+    if (type == NetworkPortType::UDP_SERVER && have_received) {
+        // connect the socket to the last receive address if we have one
+        uint32_t last_addr = 0;
+        uint16_t last_port = 0;
+        if (sock->last_recv_address(last_addr, last_port)) {
+            // we might be disconnected and want to reconnect to a different address/port
+            // if we haven't received anything for a while
+            bool maybe_disconnected = (AP_HAL::millis() - last_udp_srv_recv_time_ms) > 3000 &&
+                                      ((last_addr != last_udp_connect_address) || (last_port != last_udp_connect_port));
+            if (maybe_disconnected || !connected) {
+                char last_addr_str[IP4_STR_LEN];
+                sock->inet_addr_to_str(last_addr, last_addr_str, sizeof(last_addr_str));
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", unsigned(state.idx), last_addr_str, unsigned(last_port));
+                connected = true;
+                last_udp_connect_address = last_addr;
+                last_udp_connect_port = last_port;
+            }
+            // if we received something from the same address, reset the timer
+            if (((last_addr == last_udp_connect_address) && (last_port == last_udp_connect_port))) {
+                last_udp_srv_recv_time_ms = AP_HAL::millis();
+            }
+        }
+    }
+
     if (connected) {
         // handle outgoing packets
         uint32_t available;
@@ -353,7 +377,7 @@ bool AP_Networking::Port::send_receive(void)
             WITH_SEMAPHORE(sem);
             available = writebuffer->available();
             available = MIN(300U, available);
-#if HAL_GCS_ENABLED
+#if AP_MAVLINK_PACKETISE_ENABLED
             if (packetise) {
                 available = mavlink_packetise(*writebuffer, available);
             }
@@ -368,23 +392,27 @@ bool AP_Networking::Port::send_receive(void)
             WITH_SEMAPHORE(sem);
             n = writebuffer->peekbytes(buf, available);
         }
-        if (n > 0) {
-            const auto ret = sock->send(buf, n);
-            if (ret > 0) {
-                WITH_SEMAPHORE(sem);
-                writebuffer->advance(ret);
-                active = true;
-            }
+
+        // nothing to send return
+        if (n <= 0) {
+            return active;
         }
-    } else {
-        if (type == NetworkPortType::UDP_SERVER && have_received) {
-            // connect the socket to the last receive address if we have one
-            char buf[16];
-            uint16_t last_port;
-            const char *last_addr = sock->last_recv_address(buf, sizeof(buf), last_port);
-            if (last_addr != nullptr && port != 0) {
-                connected = sock->connect(last_addr, last_port);
+
+        ssize_t ret = -1;
+        if (type == NetworkPortType::UDP_SERVER) {
+            // UDP Server uses sendto, allowing us to change the destination address port on the fly
+            if(last_udp_connect_address != 0 && last_udp_connect_port != 0) {
+                ret = sock->sendto(buf, n, last_udp_connect_address, last_udp_connect_port);
             }
+        } else {
+            // TCP Server and Client and UDP Client use send
+            ret = sock->send(buf, n);
+        }
+
+        if (ret > 0) {
+            WITH_SEMAPHORE(sem);
+            writebuffer->advance(ret);
+            active = true;
         }
     }