diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 026b66969d..eaa51ffc8e 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -270,7 +270,7 @@ private: 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 index c4c2247b7b..832e96de1c 100644 --- a/libraries/AP_Networking/AP_Networking_port.cpp +++ b/libraries/AP_Networking/AP_Networking_port.cpp @@ -350,7 +350,11 @@ 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)); @@ -358,6 +362,10 @@ bool AP_Networking::Port::send_receive(void) 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(); + } } }