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

…ame run
ArduPilot · Feb 7, 2024 · 1f722c0 · 1f722c0
1 parent 8d6a9c0
commit 1f722c0
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 11 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;
+        char last_udp_connect_address[16];
+        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
@@ -353,7 +353,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,22 +368,26 @@ 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 && strlen(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);
+    }
+    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 = 0;
+        const char *last_addr = sock->last_recv_address(buf, sizeof(buf), last_port);
+        if (last_addr != nullptr && last_port != 0) {
+            if (!connected || strcmp(last_addr, last_udp_connect_address) != 0 || (last_port != last_udp_connect_port)) {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", unsigned(state.idx), last_addr, unsigned(last_port));
+                connected = true;
+                memcpy(last_udp_connect_address, last_addr, sizeof(buf));
+                last_udp_connect_port = last_port;
             }
         }
     }