124 lines
3.1 KiB
C++
124 lines
3.1 KiB
C++
#include "ArduinoParticipant.h"
|
|
|
|
#if defined(ARDUINO_ARCH_ESP8266)
|
|
#define HAS_WIFI 1
|
|
#include <ESP8266WiFi.h>
|
|
|
|
#elif defined(ESP32)
|
|
#define HAS_WIFI 1
|
|
#include <WiFi.h>
|
|
|
|
#elif defined(UNO_R4)
|
|
#define HAS_WIFI 1
|
|
#include <WiFi.h>
|
|
|
|
#elif defined(ARDUINO_ARCH_RP2040) // not functional, for future use
|
|
#define HAS_WIFI 1
|
|
#include <WifiNINA.h>
|
|
|
|
#endif
|
|
|
|
namespace RoboidControl {
|
|
namespace Arduino {
|
|
|
|
#if HAS_WIFI
|
|
WiFiUDP udp;
|
|
#endif
|
|
|
|
void ParticipantUDP::Setup() {
|
|
#if defined(ARDUINO) && defined(HAS_WIFI)
|
|
GetBroadcastAddress();
|
|
|
|
#if defined(UNO_R4)
|
|
if (WiFi.status() == WL_NO_MODULE) {
|
|
std::cout << "No network available!\n";
|
|
return;
|
|
}
|
|
#else
|
|
if (WiFi.isConnected() == false) {
|
|
std::cout << "No network available!\n";
|
|
return;
|
|
}
|
|
#endif
|
|
std::cout << "starting udp \n";
|
|
|
|
udp.begin(this->port);
|
|
|
|
std::cout << "Wifi sync started local " << this->port << ", remote "
|
|
<< this->remoteSite->ipAddress << ":" << this->remoteSite->port
|
|
<< "\n";
|
|
#endif
|
|
}
|
|
|
|
void ParticipantUDP::GetBroadcastAddress() {
|
|
#if defined(ARDUINO) && defined(HAS_WIFI)
|
|
IPAddress broadcastAddress = WiFi.localIP();
|
|
broadcastAddress[3] = 255;
|
|
String broadcastIpString = broadcastAddress.toString();
|
|
this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
|
|
broadcastIpString.toCharArray(this->broadcastIpAddress,
|
|
broadcastIpString.length() + 1);
|
|
std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
|
|
#endif
|
|
}
|
|
|
|
void ParticipantUDP::Receive() {
|
|
#if defined(ARDUINO) && defined(HAS_WIFI)
|
|
int packetSize = udp.parsePacket();
|
|
while (packetSize > 0) {
|
|
udp.read(buffer, packetSize);
|
|
|
|
String senderAddress = udp.remoteIP().toString();
|
|
char sender_ipAddress[16];
|
|
senderAddress.toCharArray(sender_ipAddress, 16);
|
|
unsigned int sender_port = udp.remotePort();
|
|
|
|
// std::cout << "receiving " << packetSize << " bytes, msgId "
|
|
// << (int)this->buffer[0] << "\n";
|
|
ReceiveData(packetSize, sender_ipAddress, sender_port);
|
|
|
|
packetSize = udp.parsePacket();
|
|
}
|
|
#endif // ARDUINO && HAS_WIFI
|
|
}
|
|
|
|
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
|
|
#if defined(ARDUINO) && defined(HAS_WIFI)
|
|
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
|
|
// << remoteParticipant->port << "\n";
|
|
|
|
int n = 0;
|
|
do {
|
|
if (n > 0) {
|
|
std::cout << "Retry sending\n";
|
|
delay(10);
|
|
}
|
|
n++;
|
|
udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
|
|
udp.write((unsigned char*)buffer, bufferSize);
|
|
} while (udp.endPacket() == 0 && n < 10);
|
|
|
|
#endif // ARDUINO && HAS_WIFI
|
|
return true;
|
|
}
|
|
|
|
bool ParticipantUDP::Publish(IMessage* msg) {
|
|
#if defined(ARDUINO) && defined(HAS_WIFI)
|
|
// std::cout << "Publish to " << this->broadcastIpAddress << ":"
|
|
// << this->remoteSite->port << "\n";
|
|
|
|
int bufferSize = msg->Serialize((char*)this->buffer);
|
|
if (bufferSize <= 0)
|
|
return true;
|
|
|
|
udp.beginPacket(this->broadcastIpAddress, this->remoteSite->port);
|
|
udp.write((unsigned char*)buffer, bufferSize);
|
|
udp.endPacket();
|
|
|
|
#endif
|
|
return true;
|
|
};
|
|
|
|
} // namespace Arduino
|
|
} // namespace RoboidControl
|