Fix crash at udp.begin()

Arduino/ArduinoParticipant.cpp

@@ -19,12 +19,10 @@
 namespace RoboidControl {
 namespace Arduino {
 
-void ParticipantUDP::Setup(int localPort,
-                             const char* remoteIpAddress,
-                             int remotePort) {
+WiFiUDP* udp;
+
+void ParticipantUDP::Setup() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  this->remoteIpAddress = remoteIpAddress;
-  this->remotePort = remotePort;
   GetBroadcastAddress();
 
 #if defined(UNO_R4)
@@ -38,9 +36,14 @@ void ParticipantUDP::Setup(int localPort,
     return;
   }
 #endif
-  udp.begin(localPort);
 
-  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
+  udp = new WiFiUDP();
+  udp->begin(this->port);
+
+  std::cout << "Wifi sync started local " << this->port;
+  if (this->remoteSite != nullptr)
+    std::cout << ", remote " << this->remoteSite->ipAddress << ":"
+              << this->remoteSite->port << "\n";
 #endif
 }
 
@@ -58,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) {
@@ -81,7 +84,7 @@ void ParticipantUDP::Receive() {
 
     // ReceiveData(packetSize, remoteParticipant);
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    packetSize = udp.parsePacket();
+    packetSize = udp->parsePacket();
   }
 #endif
 }
@@ -98,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;
@@ -112,9 +115,9 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   if (bufferSize <= 0)
     return true;
 
-  udp.beginPacket(this->broadcastIpAddress, this->remotePort);
-  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";

Arduino/ArduinoParticipant.h

@@ -2,29 +2,19 @@
 
 #include "Participants/ParticipantUDP.h"
 
-#if defined(HAS_WIFI)
-#include <WiFiUdp.h>
-#endif
-
 namespace RoboidControl {
 namespace Arduino {
 
 class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
-  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Setup();
   void Receive();
   bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:
-#if defined(HAS_WIFI)
-  const char* remoteIpAddress = nullptr;
-  unsigned short remotePort = 0;
   char* broadcastIpAddress = nullptr;
 
-  WiFiUDP udp;
-#endif
-
   void GetBroadcastAddress();
 };
 

Arduino/Things/DRV8833.cpp

@@ -5,18 +5,27 @@
 namespace RoboidControl {
 namespace Arduino {
 
-DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
+#if (ESP32)
+uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
+#endif
+
+DRV8833Motor::DRV8833Motor(Participant* participant,
+                           unsigned char pinIn1,
+                           unsigned char pinIn2,
+                           bool reverse)
     : Thing(participant) {
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
 
 #if (ESP32)
-  in1Ch = nextAvailablePwmChannel++;
+  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
   ledcSetup(in1Ch, 500, 8);
   ledcAttachPin(pinIn1, in1Ch);
-  in2Ch = nextAvailablePwmChannel++;
+
+  in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
   ledcSetup(in2Ch, 500, 8);
   ledcAttachPin(pinIn2, in2Ch);
+
 #else
   pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
   pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
@@ -47,7 +56,8 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
   if (this->reverse)
     motorSpeed = -motorSpeed;
 
-  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
+  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm
+  // " << rpm
   //           << ", motor signal = " << (int)motorSignal << "\n";
 
 #if (ESP32)

Arduino/Things/DRV8833.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <Arduino.h>
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
 
@@ -16,7 +17,10 @@ class DRV8833Motor : public Thing {
   /// @param pinIn1 the pin number for the in1 signal
   /// @param pinIn2 the pin number for the in2 signal
   /// @param direction the forward turning direction of the motor
-  DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false);
+  DRV8833Motor(Participant* participant,
+               unsigned char pinIn1,
+               unsigned char pinIn2,
+               bool reverse = false);
   void SetMaxRPM(unsigned int rpm);
 
   virtual void SetAngularVelocity(Spherical velocity) override;
@@ -27,6 +31,12 @@ class DRV8833Motor : public Thing {
   unsigned char pinIn1 = 255;
   unsigned char pinIn2 = 255;
   unsigned int maxRpm = 200;
+
+#if (ESP32)
+  uint8_t in1Ch;
+  uint8_t in2Ch;
+  static uint8_t nextAvailablePwmChannel;
+#endif
 };
 
 class DRV8833 : public Thing {

LinearAlgebra/Matrix.cpp

@@ -1,5 +1,7 @@
 #include "Matrix.h"
+#if !defined(NO_STD) 
 #include <iostream>
+#endif
 
 namespace LinearAlgebra {
 
@@ -61,7 +63,9 @@ Matrix2::Matrix2(const Matrix2& m)
     this->data = nullptr;
   else {
     this->data = new float[this->nValues];
-    std::copy(m.data, m.data + nValues, this->data);
+
+    for (int ix = 0; ix < this->nValues; ++ix)
+      this->data[ix] = m.data[ix];
   }
 }
 
@@ -76,7 +80,8 @@ Matrix2& Matrix2::operator=(const Matrix2& m) {
       this->data = nullptr;
     else {
       this->data = new float[this->nValues];
-      std::copy(m.data, m.data + this->nValues, this->data);
+      for (int ix = 0; ix < this->nValues; ++ix)
+        this->data[ix] = m.data[ix];
     }
   }
   return *this;
@@ -89,7 +94,8 @@ Matrix2::~Matrix2() {
 
 Matrix2 Matrix2::Clone() const {
   Matrix2 r = Matrix2(this->nRows, this->nCols);
-  std::copy(this->data, this->data + this->nValues, r.data);
+  for (int ix = 0; ix < this->nValues; ++ix)
+    r.data[ix] = this->data[ix];
   return r;
 }
 
@@ -158,8 +164,8 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
 Matrix2 Matrix2::Transpose() const {
   Matrix2 r = Matrix2(this->nCols, this->nRows);
 
-  for (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
-    for (uint colIx = 0; colIx < this->nCols; colIx++)
+  for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (int colIx = 0; colIx < this->nCols; colIx++)
       r.data[colIx * this->nCols + rowIx] =
           this->data[rowIx * this->nCols + colIx];
   }

Messages/BinaryMsg.cpp

@@ -7,6 +7,7 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
   this->thingId = thing->id;
   this->thing = thing;
   unsigned char ix = BinaryMsg::length;
+  this->data = new char[255];
   this->dataLength = this->thing->GenerateBinary(this->data, &ix);
 }
 

Participants/ParticipantUDP.cpp

@@ -69,7 +69,7 @@ void ParticipantUDP::SetupUDP(int localPort,
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
       static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+  thisArduino->Setup();
 #elif defined(IDF_VER)
   EspIdf::ParticipantUDP* thisEspIdf =
       static_cast<EspIdf::ParticipantUDP*>(this);
@@ -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);
@@ -255,8 +259,10 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize,
   Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
   if (sender == nullptr) {
     sender = this->AddParticipant(senderIpAddress, senderPort);
+#if !defined(NO_STD)
     std::cout << "New remote participant " << sender->ipAddress << ":"
               << sender->port << std::endl;
+#endif
   }
 
   ReceiveData(packetSize, sender);
@@ -317,9 +323,11 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
     } break;
   };
 
-  // Check if the buffer has been read completely
+// Check if the buffer has been read completely
+#if !defined(NO_STD)
   if (bufferSize > 0)
     std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
+#endif
 }
 
 void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
@@ -353,7 +361,8 @@ void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
 void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
-            << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << (int)msg->parentId << "\n";
+            << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
+            << (int)msg->parentId << "\n";
 #endif
 }
 
@@ -380,9 +389,13 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
     thingName[nameLength] = '\0';
     thing->name = thingName;
 
+#if !defined(NO_STD)
     std::cout << thing->name;
+#endif
   }
+#if !defined(NO_STD)
   std::cout << std::endl;
+#endif
 }
 
 void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
@@ -408,11 +421,13 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
   Thing* thing = sender->Get(msg->thingId);
   if (thing != nullptr)
     thing->ProcessBinary(msg->data);
+#if !defined(NO_STD)
   else {
     std::cout << "  unknown thing [" << (int)msg->networkId << "/"
               << (int)msg->thingId << "]";
   }
   std::cout << std::endl;
+#endif
 }
 
 // Receive