From afc48a1438f7719acf47ac191757749eba4b7171 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Apr 2025 12:36:23 +0200 Subject: [PATCH] Receiving works --- EspIdf/EspIdfParticipant.cpp | 116 ++++++++++++----------------------- EspIdf/EspIdfParticipant.h | 4 ++ LocalParticipant.cpp | 14 ++--- 3 files changed, 51 insertions(+), 83 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 2e59974..f61307d 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -1,28 +1,10 @@ #include "EspIdfParticipant.h" #include "esp_wifi.h" -#include "lwip/sockets.h" namespace RoboidControl { namespace EspIdf { -void set_socket_blocking(int sock) { - int flags = fcntl(sock, F_GETFL, 0); - if (flags != -1) { - fcntl( - sock, F_SETFL, - flags & ~O_NONBLOCK); // Clear O_NONBLOCK flag to set to blocking mode - } -} - -void set_socket_non_blocking(int sock) { - int flags = fcntl(sock, F_GETFL, 0); - if (flags != -1) { - fcntl(sock, F_SETFL, - flags | O_NONBLOCK); // Set socket to non-blocking mode - } -} - void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) { @@ -44,16 +26,6 @@ void LocalParticipant::Setup(int localPort, return; } - // // Set socket to non-blocking mode - // int flags = fcntl(sockfd, F_GETFL, 0); - // if (flags < 0) { - // std::cout << "fcntl failed"; - // close(sockfd); - // return; - // } - // fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); - // set_socket_non_blocking(sockfd); - // Set up the server address structure struct sockaddr_in local_addr; memset(&local_addr, 0, sizeof(local_addr)); @@ -62,6 +34,7 @@ void LocalParticipant::Setup(int localPort, local_addr.sin_addr.s_addr = htonl(INADDR_ANY); // Listen on all available network interfaces + // Bind the socket to the address and port if (bind(sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) { std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; @@ -70,6 +43,13 @@ void LocalParticipant::Setup(int localPort, return; } + // struct sockaddr_in dest_addr; + memset(dest_addr.sin_zero, 0, sizeof(dest_addr.sin_zero)); + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(this->remoteSite->port); + inet_pton(AF_INET, this->remoteSite->ipAddress, &dest_addr.sin_addr.s_addr); + + std::cout << "Wifi sync started local " << this->port << ", remote " << this->remoteSite->ipAddress << ":" << this->remoteSite->port << "\n"; @@ -99,36 +79,39 @@ void LocalParticipant::GetBroadcastAddress() { void LocalParticipant::Receive() { #if defined(IDF_VER) -return; // disable receiving for now - set_socket_non_blocking(sockfd); + struct pollfd fds; + fds.fd = sockfd; + fds.events = POLLIN; // We're looking for data available to read - struct sockaddr_in client_addr; - socklen_t addr_len = sizeof(client_addr); - int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, - (struct sockaddr*)&client_addr, &addr_len); - while (packetSize > 0) { - char sender_ipAddress[16]; - inet_ntoa_r(client_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); - unsigned int sender_port = client_addr.sin_port; - - // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, - // sender_port); if (remoteParticipant == nullptr) { - // remoteParticipant = this->AddParticipant(sender_ipAddress, - // sender_port); - // // std::cout << "New sender " << sender_ipAddress << ":" << sender_port - // // << "\n"; - // // std::cout << "New remote participant " << - // remoteParticipant->ipAddress - // // << ":" << remoteParticipant->port << " " - // // << (int)remoteParticipant->networkId << "\n"; - // } - - // ReceiveData(packetSize, remoteParticipant); - ReceiveData(packetSize, sender_ipAddress, sender_port); - packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, - (struct sockaddr*)&client_addr, &addr_len); + // Use poll() with a timeout of 0 to return immediately + int ret = poll(&fds, 1, 0); + if (ret == -1) { + std::cout << "poll() error\n"; + return; + } + socklen_t addr_len = sizeof(this->src_addr); + + while (ret > 0 && fds.revents & POLLIN) { + int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, + (struct sockaddr*)&this->src_addr, &addr_len); + if (packetSize < 0) { + std::cout << "recvfrom() error\n"; + return; + } + + char sender_ipAddress[16]; + inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); + unsigned int sender_port = ntohs(this->src_addr.sin_port); + std::cout << "Received from " << ntohs(this->src_addr.sin_port) << "\n"; + + ReceiveData(packetSize, sender_ipAddress, sender_port); + + int ret = poll(&fds, 1, 0); + if (ret == -1) { + std::cout << "poll() error\n"; + return; + } } - set_socket_blocking(sockfd); #endif } @@ -138,26 +121,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { std::cout << "Sending to " << remoteParticipant->ipAddress << ":" << remoteParticipant->port << "\n"; - struct sockaddr_in dest_addr; - memset(dest_addr.sin_zero, 0, sizeof(dest_addr.sin_zero)); - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(remoteParticipant->port); - inet_pton(AF_INET, remoteParticipant->ipAddress, &dest_addr.sin_addr.s_addr); - - int err = 0; - // int n = 0; - // do { - // if (n > 0) { - // std::cout << "Retry sending\n"; - // vTaskDelay(pdMS_TO_TICKS(10)); // Wait 10ms - // } - // n++; - // err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, - // sizeof(dest_addr)); - - // } while (errno == EAGAIN && n < 10); - - err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, + int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); if (errno != 0) std::cout << "Send error " << err << " or " << errno << "\n"; diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index b76dafe..9a1c549 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -2,6 +2,8 @@ #include "../LocalParticipant.h" +#include "lwip/sockets.h" + namespace RoboidControl { namespace EspIdf { @@ -18,6 +20,8 @@ class LocalParticipant : public RoboidControl::LocalParticipant { char* broadcastIpAddress = nullptr; int sockfd; + struct sockaddr_in dest_addr; + struct sockaddr_in src_addr; void GetBroadcastAddress(); }; diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index d898009..1d94e55 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -260,11 +260,11 @@ void LocalParticipant::ReceiveData(unsigned char packetSize, this->GetParticipant(senderIpAddress, senderPort); if (remoteParticipant == nullptr) { remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); - // std::cout << "New sender " << sender_ipAddress << ":" << sender_port - // << "\n"; - // std::cout << "New remote participant " << remoteParticipant->ipAddress - // << ":" << remoteParticipant->port << " " - // << (int)remoteParticipant->networkId << "\n"; + std::cout << "New sender " << senderIpAddress << ":" << senderPort + << "\n"; + std::cout << "New remote participant " << remoteParticipant->ipAddress + << ":" << remoteParticipant->port << " " + << (int)remoteParticipant->networkId << "\n"; } ReceiveData(packetSize, remoteParticipant); @@ -316,8 +316,8 @@ void LocalParticipant::ReceiveData(unsigned char bufferSize, void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {} void LocalParticipant::Process(Participant* sender, SiteMsg* msg) { - // std::cout << this->name << ": process NetworkId [" << (int)this->networkId - // << "/" << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process NetworkId [" << (int)this->networkId + << "/" << (int)msg->networkId << "]\n"; if (this->networkId != msg->networkId) { this->networkId = msg->networkId; // std::cout << this->things.size() << " things\n";