From 8ff0fdd78f6cb4c4f44eb3cc96c750d53bc0b48f Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Fri, 18 Apr 2025 14:36:07 +0200
Subject: [PATCH] Fix crash at udp.begin()

---
 Arduino/ArduinoParticipant.cpp  | 28 +++++++++++++++-------------
 Arduino/ArduinoParticipant.h    |  8 --------
 Participants/ParticipantUDP.cpp |  8 ++++++--
 3 files changed, 21 insertions(+), 23 deletions(-)

diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp
index c0e4afe..f74a746 100644
--- a/Arduino/ArduinoParticipant.cpp
+++ b/Arduino/ArduinoParticipant.cpp
@@ -19,6 +19,8 @@
 namespace RoboidControl {
 namespace Arduino {
 
+WiFiUDP* udp;
+
 void ParticipantUDP::Setup() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   GetBroadcastAddress();
@@ -34,9 +36,9 @@ void ParticipantUDP::Setup() {
     return;
   }
 #endif
-  std::cout << "UDP.begin " << this->port << std::endl;
 
-  udp.begin(this->port);
+  udp = new WiFiUDP();
+  udp->begin(this->port);
 
   std::cout << "Wifi sync started local " << this->port;
   if (this->remoteSite != nullptr)
@@ -59,14 +61,14 @@ void ParticipantUDP::GetBroadcastAddress() {
 
 void ParticipantUDP::Receive() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  int packetSize = udp.parsePacket();
+  int packetSize = udp->parsePacket();
   while (packetSize > 0) {
-    udp.read(buffer, packetSize);
+    udp->read(buffer, packetSize);
 
-    String senderAddress = udp.remoteIP().toString();
+    String senderAddress = udp->remoteIP().toString();
     char sender_ipAddress[16];
     senderAddress.toCharArray(sender_ipAddress, 16);
-    unsigned int sender_port = udp.remotePort();
+    unsigned int sender_port = udp->remotePort();
 
     // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
     // sender_port); if (remoteParticipant == nullptr) {
@@ -82,7 +84,7 @@ void ParticipantUDP::Receive() {
 
     // ReceiveData(packetSize, remoteParticipant);
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    packetSize = udp.parsePacket();
+    packetSize = udp->parsePacket();
   }
 #endif
 }
@@ -99,9 +101,9 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
       delay(10);
     }
     n++;
-    udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
-    udp.write((unsigned char*)buffer, bufferSize);
-  } while (udp.endPacket() == 0 && n < 10);
+    udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
+    udp->write((unsigned char*)buffer, bufferSize);
+  } while (udp->endPacket() == 0 && n < 10);
 
 #endif
   return true;
@@ -113,9 +115,9 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   if (bufferSize <= 0)
     return true;
 
-  udp.beginPacket(this->broadcastIpAddress, this->port);
-  udp.write((unsigned char*)buffer, bufferSize);
-  udp.endPacket();
+  udp->beginPacket(this->broadcastIpAddress, this->port);
+  udp->write((unsigned char*)buffer, bufferSize);
+  udp->endPacket();
 
   //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
   //             << this->remotePort << "\n";
diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h
index da387be..5a9396f 100644
--- a/Arduino/ArduinoParticipant.h
+++ b/Arduino/ArduinoParticipant.h
@@ -2,10 +2,6 @@
 
 #include "Participants/ParticipantUDP.h"
 
-#if defined(HAS_WIFI)
-#include <WiFiUdp.h>
-#endif
-
 namespace RoboidControl {
 namespace Arduino {
 
@@ -17,12 +13,8 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
   bool Publish(IMessage* msg);
 
  protected:
-#if defined(HAS_WIFI)
   char* broadcastIpAddress = nullptr;
 
-  WiFiUDP udp;
-#endif
-
   void GetBroadcastAddress();
 };
 
diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp
index 0783a40..65e28d9 100644
--- a/Participants/ParticipantUDP.cpp
+++ b/Participants/ParticipantUDP.cpp
@@ -104,14 +104,15 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
     if (thing == nullptr)
       continue;
 
-    if (this->isIsolated == false) {
+    thing->Update(currentTimeMs, true);
+
+    if (this->isIsolated == false && thing->owner != this) {
       PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
       this->Send(thing->owner, poseMsg);
       BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
       this->Send(thing->owner, binaryMsg);
       delete poseMsg;
     }
-    thing->Update(currentTimeMs, true);
   }
 }
 
@@ -178,6 +179,8 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
 }
 
 bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
+  // std::cout << "send msg " << (int)this->buffer[0] << " to "
+  //           << remoteParticipant->ipAddress << std::endl;
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)
     return true;
@@ -224,6 +227,7 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) {
 }
 
 bool ParticipantUDP::Publish(IMessage* msg) {
+  // std::cout << "publish msg\n";
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =
       static_cast<Windows::ParticipantUDP*>(this);