From 8130a057eabe3c560a84c4664abf2389c90f58f4 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Apr 2025 11:41:33 +0200 Subject: [PATCH] Sending messages works --- EspIdf/EspIdfParticipant.cpp | 98 +++++++++++++++++++++++++----------- EspIdf/EspIdfParticipant.h | 4 +- LocalParticipant.cpp | 2 +- 3 files changed, 71 insertions(+), 33 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 9f8004c..2e59974 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -6,12 +6,27 @@ 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) { #if defined(IDF_VER) - this->remoteIpAddress = remoteIpAddress; - this->remotePort = remotePort; GetBroadcastAddress(); wifi_ap_record_t ap_info; @@ -21,11 +36,6 @@ void LocalParticipant::Setup(int localPort, return; } - struct sockaddr_in server_addr; //, client_addr; - // socklen_t addr_len = sizeof(client_addr); - // char recv_buffer[1024]; - // int sockfd; - // Create a UDP socket sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { @@ -34,31 +44,35 @@ 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 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 - memset(&server_addr, 0, sizeof(server_addr)); - server_addr.sin_family = AF_INET; - server_addr.sin_port = htons(localPort); - server_addr.sin_addr.s_addr = + struct sockaddr_in local_addr; + memset(&local_addr, 0, sizeof(local_addr)); + local_addr.sin_family = AF_INET; + local_addr.sin_port = htons(this->port); + 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*)&server_addr, sizeof(server_addr)) < 0) { + if (bind(sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) { std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; close(sockfd); vTaskDelete(NULL); return; } - std::cout << "Wifi sync started to port " << this->remotePort << "\n"; + std::cout << "Wifi sync started local " << this->port << ", remote " + << this->remoteSite->ipAddress << ":" << this->remoteSite->port + << "\n"; #endif } @@ -72,15 +86,22 @@ void LocalParticipant::GetBroadcastAddress() { return; } - ip_addr_t broadcast_addr; + ip_addr_t broadcast_addr = {}; broadcast_addr.u_addr.ip4.addr = (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr; - std::cout << "Broadcast address: " << broadcastIpAddress << "\n"; + this->broadcastIpAddress = new char[16]; // IPv4 address can have a max of 15 + // characters + null terminator + snprintf(this->broadcastIpAddress, 16, IPSTR, + IP2STR(&broadcast_addr.u_addr.ip4)); + std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n"; } void LocalParticipant::Receive() { #if defined(IDF_VER) +return; // disable receiving for now + set_socket_non_blocking(sockfd); + struct sockaddr_in client_addr; socklen_t addr_len = sizeof(client_addr); int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, @@ -107,22 +128,39 @@ void LocalParticipant::Receive() { packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, (struct sockaddr*)&client_addr, &addr_len); } + set_socket_blocking(sockfd); + #endif } bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) - // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" - // << remoteParticipant->port << "\n"; + 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 = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, - sizeof(dest_addr)); - if (err != 0) - std::cout << "Send error\n"; + + 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, + sizeof(dest_addr)); + if (errno != 0) + std::cout << "Send error " << err << " or " << errno << "\n"; #endif return true; @@ -136,7 +174,7 @@ bool LocalParticipant::Publish(IMessage* msg) { struct sockaddr_in dest_addr; dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(this->remotePort); + dest_addr.sin_port = htons(this->port); inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr); int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index 2d90d2e..b76dafe 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -13,8 +13,8 @@ class LocalParticipant : public RoboidControl::LocalParticipant { bool Publish(IMessage* msg); protected: - const char* remoteIpAddress = nullptr; - unsigned short remotePort = 0; + // const char* remoteIpAddress = nullptr; + // unsigned short remotePort = 0; char* broadcastIpAddress = nullptr; int sockfd; diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index 5ebf3a6..d898009 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -56,7 +56,7 @@ void LocalParticipant::begin() { if (this->isIsolated) return; - SetupUDP(this->port, this->ipAddress, this->port); + SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); } void LocalParticipant::SetupUDP(int localPort,