aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Networking: allow UDP Server to talk to different clients on the same run

Browse Source
This commit is contained in:
bugobliterator 2024-02-07 16:27:35 +11:00 committed by Andrew Tridgell
parent bace8d4c85
commit a82729589c
2 changed files with 38 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Networking/AP_Networking.h
View File

@ -266,6 +266,8 @@ private:
uint32_t last_size_rx; uint32_t last_size_rx;
bool packetise; bool packetise;
bool connected; bool connected;
uint32_t last_udp_connect_address;
uint16_t last_udp_connect_port;
bool have_received; bool have_received;
bool close_on_recv_error; bool close_on_recv_error;

52
libraries/AP_Networking/AP_Networking_port.cpp
View File

@ -345,6 +345,22 @@ 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;
if (sock->last_recv_address(last_addr, last_port)) {
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) { if (connected) {
// handle outgoing packets // handle outgoing packets
uint32_t available; uint32_t available;
@ -353,7 +369,7 @@ bool AP_Networking::Port::send_receive(void)
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
available = writebuffer->available(); available = writebuffer->available();
available = MIN(300U, available); available = MIN(300U, available);
#if HAL_GCS_ENABLED #if AP_MAVLINK_PACKETISE_ENABLED
if (packetise) { if (packetise) {
available = mavlink_packetise(*writebuffer, available); available = mavlink_packetise(*writebuffer, available);
} }
@ -368,23 +384,27 @@ bool AP_Networking::Port::send_receive(void)
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
n = writebuffer->peekbytes(buf, available); n = writebuffer->peekbytes(buf, available);
} }
if (n > 0) {
const auto ret = sock->send(buf, n); // nothing to send return
if (ret > 0) { if (n <= 0) {
WITH_SEMAPHORE(sem); return active;
writebuffer->advance(ret);
active = true;
}
} }
} else {
if (type == NetworkPortType::UDP_SERVER && have_received) { ssize_t ret = -1;
// connect the socket to the last receive address if we have one if (type == NetworkPortType::UDP_SERVER) {
char buf[16]; // UDP Server uses sendto, allowing us to change the destination address port on the fly
uint16_t last_port; if(last_udp_connect_address != 0 && last_udp_connect_port != 0) {
const char *last_addr = sock->last_recv_address(buf, sizeof(buf), last_port); ret = sock->sendto(buf, n, last_udp_connect_address, last_udp_connect_port);
if (last_addr != nullptr && port != 0) {
connected = sock->connect(last_addr, last_port);
} }
} else {
// TCP Server and Client and UDP Client use send
ret = sock->send(buf, n);
}
if (ret > 0) {
WITH_SEMAPHORE(sem);
writebuffer->advance(ret);
active = true;
} }
} }