Merge commit '61740913fec4289924f5c485556b8af17bc44e1b'

ArduinoUtils.cpp

@@ -2,6 +2,7 @@
 #if defined(ARDUINO)
 #include <Arduino.h>
 #include <ESP8266WiFi.h>
+#include <ESP8266httpUpdate.h>
 
 bool StartWifi(const char *wifiSsid, const char *wifiPassword,
                bool hotspotFallback) {

ArduinoUtils.h

@@ -1,4 +1,10 @@
 #pragma once
+#if defined(ARDUINO)
 
+#include <Arduino.h>
 bool StartWifi(const char *wifiSsid, const char *wifiPassword,
                bool hotspotFallback);
+
+void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION);
+
+#endif

Messages.cpp

@@ -33,94 +33,3 @@ unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
 
 // IMessage
 #pragma endregion
-
-#pragma region Investigate
-
-InvestigateMsg::InvestigateMsg(char *buffer) {
-  unsigned ix = 1; // first byte is msgId
-  this->networkId = buffer[ix++];
-  this->thingId = buffer[ix++];
-}
-InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
-  this->networkId = networkId;
-  this->thingId = thingId;
-}
-
-unsigned char InvestigateMsg::Serialize(char *buffer) {
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  buffer[ix++] = this->thingId;
-  return ix;
-}
-
-// bool InvestigateMsg::Send(Participant *participant, unsigned char networkId,
-//                           unsigned char thingId) {
-//   InvestigateMsg msg = InvestigateMsg(networkId, thingId);
-//   return msg.Send(participant);
-// }
-
-// Investigate
-#pragma endregion
-
-#pragma region PoseMsg
-
-PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
-                 unsigned char poseType, Spherical16 position,
-                 SwingTwist16 orientation, Spherical16 linearVelocity,
-                 Spherical16 angularVelocity) {
-  this->networkId = networkId;
-  this->thingId = thingId;
-
-  this->poseType = poseType;
-  this->position = position;
-  this->orientation = orientation;
-  this->linearVelocity = linearVelocity;
-  this->angularVelocity = angularVelocity;
-}
-PoseMsg::PoseMsg(const char *buffer) {
-  unsigned char ix = 1; // First byte is msg id
-  this->networkId = buffer[ix++];
-  this->thingId = buffer[ix++];
-  this->poseType = buffer[ix++];
-  this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
-  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
-}
-
-unsigned char PoseMsg::Serialize(char *buffer) {
-  unsigned char ix = 0;
-  buffer[ix++] = PoseMsg::id;
-  buffer[ix++] = this->networkId;
-  buffer[ix++] = this->thingId;
-  buffer[ix++] = this->poseType;
-  if ((this->poseType & Pose_Position) != 0)
-    LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
-  if ((this->poseType & Pose_Orientation) != 0)
-    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
-  if ((this->poseType & Pose_LinearVelocity) != 0)
-    LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
-  if ((this->poseType & Pose_AngularVelocity) != 0)
-    LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
-  return ix;
-}
-
-// Pose
-#pragma endregion
-
-#pragma region DestroyMsg
-
-DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
-  this->networkId = networkId;
-  this->thingId = thing->id;
-}
-
-unsigned char DestroyMsg::Serialize(char *buffer) {
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  buffer[ix++] = this->thingId;
-  return ix;
-}
-
-// DestroyMsg
-#pragma endregion

Messages.h

@@ -21,59 +21,6 @@ public:
   // bool SendTo(Participant *participant);
 };
 
-class InvestigateMsg : public IMessage {
-public:
-  static const unsigned char id = 0x81;
-  static const unsigned char length = 3;
-  unsigned char networkId;
-  unsigned char thingId;
-
-  InvestigateMsg(char *buffer);
-  InvestigateMsg(unsigned char networkId, unsigned char thingId);
-
-  virtual unsigned char Serialize(char *buffer) override;
-};
-
-class PoseMsg : public IMessage {
-public:
-  static const unsigned char id = 0x10;
-  unsigned char length = 4 + 4 + 4;
-
-  unsigned char networkId;
-  unsigned char thingId;
-
-  unsigned char poseType;
-  static const unsigned char Pose_Position = 0x01;
-  static const unsigned char Pose_Orientation = 0x02;
-  static const unsigned char Pose_LinearVelocity = 0x04;  // For future use
-  static const unsigned char Pose_AngularVelocity = 0x08; // For future use
-
-  Spherical16 position;
-  SwingTwist16 orientation;
-  Spherical16 linearVelocity;
-  Spherical16 angularVelocity;
-
-  PoseMsg(unsigned char networkId, unsigned char thingId,
-          unsigned char poseType, Spherical16 position,
-          SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(),
-          Spherical16 angularVelocity = Spherical16());
-  PoseMsg(const char *buffer);
-
-  virtual unsigned char Serialize(char *buffer) override;
-};
-
-class DestroyMsg : public IMessage {
-public:
-  static const unsigned char id = 0x20;
-  static const unsigned length = 3;
-  unsigned char networkId;
-  unsigned char thingId;
-
-  DestroyMsg(unsigned char networkId, Thing *thing);
-
-  virtual unsigned char Serialize(char *buffer) override;
-};
-
 } // namespace Control
 } // namespace Passer
 

Messages/ClientMsg.cpp

@@ -6,6 +6,8 @@ ClientMsg::ClientMsg(char networkId) { this->networkId = networkId; }
 
 ClientMsg::ClientMsg(const char *buffer) { this->networkId = buffer[1]; }
 
+ClientMsg::~ClientMsg() {}
+
 unsigned char ClientMsg::Serialize(char *buffer) {
   unsigned char ix = 0;
   buffer[ix++] = this->id;

Messages/ClientMsg.h

@@ -16,6 +16,7 @@ public:
 
   ClientMsg(char networkId);
   ClientMsg(const char *buffer);
+  virtual ~ClientMsg();
 
   virtual unsigned char Serialize(char *buffer) override;
 };

Messages/CustomMsg.h

@@ -19,7 +19,7 @@ public:
 
   CustomMsg(char *buffer);
   CustomMsg(unsigned char networkId, Thing *thing);
-  ~CustomMsg();
+  virtual ~CustomMsg();
 
   virtual unsigned char Serialize(char *buffer) override;
 

Messages/DestroyMsg.cpp

@@ -0,0 +1,22 @@
+#include "DestroyMsg.h"
+
+namespace Passer {
+namespace Control {
+
+DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
+  this->networkId = networkId;
+  this->thingId = thing->id;
+}
+
+DestroyMsg::~DestroyMsg() {}
+
+unsigned char DestroyMsg::Serialize(char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  return ix;
+}
+
+} // namespace Control
+} // namespace Passer

Messages/DestroyMsg.h

@@ -0,0 +1,19 @@
+#include "Messages.h"
+namespace Passer {
+namespace Control {
+
+class DestroyMsg : public IMessage {
+public:
+  static const unsigned char id = 0x20;
+  static const unsigned length = 3;
+  unsigned char networkId;
+  unsigned char thingId;
+
+  DestroyMsg(unsigned char networkId, Thing *thing);
+  virtual ~DestroyMsg();
+
+  virtual unsigned char Serialize(char *buffer) override;
+};
+
+} // namespace Control
+} // namespace Passer

Messages/InvestigateMsg.cpp

@@ -0,0 +1,30 @@
+#include "InvestigateMsg.h"
+#pragma region Investigate
+
+InvestigateMsg::InvestigateMsg(char *buffer) {
+  unsigned ix = 1; // first byte is msgId
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+}
+InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
+  this->networkId = networkId;
+  this->thingId = thingId;
+}
+
+InvestigateMsg::~InvestigateMsg() {}
+unsigned char InvestigateMsg::Serialize(char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  return ix;
+}
+
+// bool InvestigateMsg::Send(Participant *participant, unsigned char networkId,
+//                           unsigned char thingId) {
+//   InvestigateMsg msg = InvestigateMsg(networkId, thingId);
+//   return msg.Send(participant);
+// }
+
+// Investigate
+#pragma endregion

Messages/InvestigateMsg.h

@@ -0,0 +1,15 @@
+#include "Messages.h"
+
+class InvestigateMsg : public IMessage {
+public:
+  static const unsigned char id = 0x81;
+  static const unsigned char length = 3;
+  unsigned char networkId;
+  unsigned char thingId;
+
+  InvestigateMsg(char *buffer);
+  InvestigateMsg(unsigned char networkId, unsigned char thingId);
+  virtual ~InvestigateMsg();
+
+  virtual unsigned char Serialize(char *buffer) override;
+};

Messages/ModelUrlMsg.cpp

@@ -35,6 +35,8 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing *thing) {
   this->url = thing->modelUrl; // dangerous!
 }
 
+ModelUrlMsg::~ModelUrlMsg() {}
+
 unsigned char ModelUrlMsg::Serialize(char *buffer) {
   if (this->urlLength == 0 || this->url == nullptr)
     return 0;

Messages/ModelUrlMsg.h

@@ -18,6 +18,7 @@ public:
   ModelUrlMsg(unsigned char networkId, Thing *thing);
   //   ModelUrlMsg(unsigned char networkId, unsigned char thingId,
   //               unsigned char urlLegth, const char *url, float scale = 1);
+  virtual ~ModelUrlMsg();
 
   virtual unsigned char Serialize(char *buffer) override;
 };

Messages/NameMsg.cpp

@@ -32,6 +32,8 @@ NameMsg::NameMsg(unsigned char networkId, Thing *thing) {
 //   this->nameLength = nameLength;
 // }
 
+NameMsg::~NameMsg() {}
+
 unsigned char NameMsg::Serialize(char *buffer) {
   if (this->nameLength == 0 || this->name == nullptr)
     return 0;

Messages/NameMsg.h

@@ -16,6 +16,7 @@ public:
   NameMsg(unsigned char networkId, Thing *thing);
   //   NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
   //           unsigned char nameLength);
+  virtual ~NameMsg();
 
   virtual unsigned char Serialize(char *buffer) override;
 };

Messages/NetworkIdMsg.cpp

@@ -9,6 +9,8 @@ NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
   this->networkId = networkId;
 }
 
+NetworkIdMsg::~NetworkIdMsg() {}
+
 unsigned char NetworkIdMsg::Serialize(char *buffer) {
   unsigned char ix = 0;
   buffer[ix++] = this->id;

Messages/NetworkIdMsg.h

@@ -11,6 +11,7 @@ public:
 
   NetworkIdMsg(const char *buffer);
   NetworkIdMsg(unsigned char networkId);
+  virtual ~NetworkIdMsg();
 
   virtual unsigned char Serialize(char *buffer) override;
   // static NetworkIdMsg Receive(char *buffer, unsigned char bufferSize);

Messages/PoseMsg.cpp

@@ -0,0 +1,43 @@
+#include "PoseMsg.h"
+#include "LowLevelMessages.h"
+
+PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
+                 unsigned char poseType, Spherical16 position,
+                 SwingTwist16 orientation, Spherical16 linearVelocity,
+                 Spherical16 angularVelocity) {
+  this->networkId = networkId;
+  this->thingId = thingId;
+
+  this->poseType = poseType;
+  this->position = position;
+  this->orientation = orientation;
+  this->linearVelocity = linearVelocity;
+  this->angularVelocity = angularVelocity;
+}
+PoseMsg::PoseMsg(const char *buffer) {
+  unsigned char ix = 1; // First byte is msg id
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->poseType = buffer[ix++];
+  this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
+  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
+}
+
+PoseMsg::~PoseMsg() {}
+
+unsigned char PoseMsg::Serialize(char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = PoseMsg::id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  buffer[ix++] = this->poseType;
+  if ((this->poseType & Pose_Position) != 0)
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
+  if ((this->poseType & Pose_Orientation) != 0)
+    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
+  if ((this->poseType & Pose_LinearVelocity) != 0)
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
+  if ((this->poseType & Pose_AngularVelocity) != 0)
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
+  return ix;
+}

Messages/PoseMsg.h

@@ -0,0 +1,30 @@
+#include "Messages.h"
+
+class PoseMsg : public IMessage {
+public:
+  static const unsigned char id = 0x10;
+  unsigned char length = 4 + 4 + 4;
+
+  unsigned char networkId;
+  unsigned char thingId;
+
+  unsigned char poseType;
+  static const unsigned char Pose_Position = 0x01;
+  static const unsigned char Pose_Orientation = 0x02;
+  static const unsigned char Pose_LinearVelocity = 0x04;  // For future use
+  static const unsigned char Pose_AngularVelocity = 0x08; // For future use
+
+  Spherical16 position;
+  SwingTwist16 orientation;
+  Spherical16 linearVelocity;
+  Spherical16 angularVelocity;
+
+  PoseMsg(unsigned char networkId, unsigned char thingId,
+          unsigned char poseType, Spherical16 position,
+          SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(),
+          Spherical16 angularVelocity = Spherical16());
+  PoseMsg(const char *buffer);
+  virtual ~PoseMsg();
+
+  virtual unsigned char Serialize(char *buffer) override;
+};

Messages/ThingMsg.cpp

@@ -30,6 +30,8 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing *thing) {
 //   this->parentId = parentId;
 // }
 
+ThingMsg::~ThingMsg() {}
+
 unsigned char ThingMsg::Serialize(char *buffer) {
   unsigned char ix = 0;
   buffer[ix++] = this->id;

Messages/ThingMsg.h

@@ -16,6 +16,7 @@ public:
   ThingMsg(unsigned char networkId, Thing *thing);
   //   ThingMsg(unsigned char networkId, unsigned char thingId,
   //            unsigned char thingType, unsigned char parentId);
+  virtual ~ThingMsg();
 
   virtual unsigned char Serialize(char *buffer) override;
 };

Participant.cpp

@@ -31,7 +31,8 @@ Participant::Participant(int port) {
   this->participants.push_back(this);
 
   int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
-  SetupUDP(randomPort, ipAddress, port);
+  this->localPort = randomPort;
+  // SetupUDP(randomPort, ipAddress, port);
 }
 
 Participant::Participant(const char *ipAddress, int port) {
@@ -41,11 +42,15 @@ Participant::Participant(const char *ipAddress, int port) {
   this->participants.push_back(this);
 
   int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
-  SetupUDP(randomPort, ipAddress, port);
+  this->localPort = randomPort;
+  // SetupUDP(randomPort, ipAddress, port);
 }
 
-void Passer::Control::Participant::SetupUDP(int localPort,
+void Participant::begin() {
-                                            const char *remoteIpAddress,
+  SetupUDP(this->localPort, this->ipAddress, this->port);
+}
+
+void Participant::SetupUDP(int localPort, const char *remoteIpAddress,
                            int remotePort) {
 #if defined(_WIN32) || defined(_WIN64)
   UdpWindows *thisWindows = static_cast<UdpWindows *>(this);
@@ -57,6 +62,7 @@ void Passer::Control::Participant::SetupUDP(int localPort,
   UdpArduino *thisArduino = static_cast<UdpArduino *>(this);
   thisArduino->Setup(localPort, remoteIpAddress, remotePort);
 #endif
+  this->connected = true;
 }
 
 void Participant::Update(unsigned long currentTimeMs) {
@@ -71,6 +77,9 @@ void Participant::Update(unsigned long currentTimeMs) {
 #endif
   }
 
+  if (this->connected == false)
+    begin();
+
   if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
     ClientMsg *msg = new ClientMsg(this->networkId);
     this->Publish(msg);
@@ -115,15 +124,15 @@ Participant *Participant::AddParticipant(const char *ipAddress, int port) {
 
 void Participant::SendThingInfo(Thing *thing) {
   std::cout << "Send thing info\n";
-  IMessage *msg = new ThingMsg(this->networkId, thing);
+  ThingMsg *thingMsg = new ThingMsg(this->networkId, thing);
-  this->Send(msg);
+  this->Send(thingMsg);
-  delete msg;
+  delete thingMsg;
-  msg = new NameMsg(this->networkId, thing);
+  NameMsg *nameMsg = new NameMsg(this->networkId, thing);
-  this->Send(msg);
+  this->Send(nameMsg);
-  delete msg;
+  delete nameMsg;
-  msg = new ModelUrlMsg(this->networkId, thing);
+  ModelUrlMsg *modelMsg = new ModelUrlMsg(this->networkId, thing);
-  this->Send(msg);
+  this->Send(modelMsg);
-  delete msg;
+  delete modelMsg;
 }
 
 void Passer::Control::Participant::PublishThingInfo(Thing *thing) {
@@ -245,8 +254,9 @@ void Participant::Process(ThingMsg *msg) {}
 void Participant::Process(NameMsg *msg) {
   Thing *thing = Thing::Get(msg->networkId, msg->thingId);
   if (thing != nullptr) {
-    thing->name = new char[strlen(msg->name)];
+    char *thingName = new char[strlen(msg->name)];
-    strcpy(thing->name, msg->name);
+    strcpy(thingName, msg->name);
+    thing->name = thingName;
     std::cout << "thing name = " << thing->name << "\n";
   }
 }

Participant.h

@@ -1,12 +1,14 @@
 #pragma once
 
-#include "ClientMsg.h"
 #include "Messages.h"
+#include "Messages/ClientMsg.h"
 #include "Messages/CustomMsg.h"
-#include "ModelUrlMsg.h"
+#include "Messages/InvestigateMsg.h"
-#include "NameMsg.h"
+#include "Messages/ModelUrlMsg.h"
-#include "NetworkIdMsg.h"
+#include "Messages/NameMsg.h"
-#include "ThingMsg.h"
+#include "Messages/NetworkIdMsg.h"
+#include "Messages/PoseMsg.h"
+#include "Messages/ThingMsg.h"
 
 #include <list>
 
@@ -35,6 +37,7 @@ public:
 
   const char *ipAddress = "0.0.0.0";
   int port = 0;
+  int localPort = 0;
 
 #if defined(ARDUINO)
   const char *remoteIpAddress = nullptr;
@@ -62,6 +65,9 @@ public:
   // i.e.
   // Participant p = Participant("127.0.0.1", 8000);
 
+  void begin();
+  bool connected = false;
+
   virtual void Update(unsigned long currentTimeMs = 0);
 
   void SendThingInfo(Thing *thing);