RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Receiving works

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-03 12:36:23 +02:00
parent 8130a057ea
commit afc48a1438
3 changed files with 51 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
EspIdf/EspIdfParticipant.cpp
View File

@ -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";

4
EspIdf/EspIdfParticipant.h
View File

@ -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();
};

14
LocalParticipant.cpp
View File

@ -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";