AP_Networking: add timeout to swap the UDP Server client target

mirkix · 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();
+ }
  }
  }