AP_Networking: allow UDP Server to talk to different clients on the s…

…ame run
bugobliterator · Feb 8, 2024 · d7b9839 · d7b9839
1 parent 9ee6c9d
commit d7b9839
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 13 deletions.
diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h
@@ -266,6 +266,8 @@ 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;
 

diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp
@@ -345,6 +345,23 @@ 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;
+        sock->last_recv_address(last_addr, last_port);
+        if (last_addr != 0 && last_port != 0) {
+            if (!connected || (last_addr != last_udp_connect_address) || (last_port != last_udp_connect_port)) {
+                char last_addr_str[16];
+                sock->inet_addr_to_str(last_addr, last_addr_str, 16);
+                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 (connected) {
         // handle outgoing packets
         uint32_t available;
@@ -353,7 +370,7 @@ bool AP_Networking::Port::send_receive(void)
             WITH_SEMAPHORE(sem);
             available = writebuffer->available();
             available = MIN(300U, available);
-#if HAL_GCS_ENABLED
+#if defined(HAL_ENABLE_MAVLINK_PACKETISE)
             if (packetise) {
                 available = mavlink_packetise(*writebuffer, available);
             }
@@ -368,24 +385,14 @@ 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 (n > 0 && last_udp_connect_address != 0 && last_udp_connect_port != 0) {
+            const auto ret = sock->sendto(buf, n, last_udp_connect_address, last_udp_connect_port);
             if (ret > 0) {
                 WITH_SEMAPHORE(sem);
                 writebuffer->advance(ret);
                 active = true;
             }
         }
-    } 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);
-            }
-        }
     }
 
     return active;