AP_Networking: add timeout to swap the UDP Server client target

ArduPilot · Jun 21, 2024 · 96682b1 · 96682b1
1 parent d07e8d1
commit 96682b1
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 2 deletions.
diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h
@@ -270,7 +270,7 @@ class AP_Networking
         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

diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp
@@ -350,14 +350,22 @@ bool AP_Networking::Port::send_receive(void)
         uint32_t last_addr = 0;
         uint16_t last_port = 0;
         if (sock->last_recv_address(last_addr, last_port)) {
-            if (!connected || (last_addr != last_udp_connect_address) || (last_port != last_udp_connect_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();
+            }
         }
     }