diff --git a/ArduinoUtils.cpp b/ArduinoUtils.cpp index 7581a0c..0de1370 100644 --- a/ArduinoUtils.cpp +++ b/ArduinoUtils.cpp @@ -2,6 +2,7 @@ #if defined(ARDUINO) #include #include +#include bool StartWifi(const char *wifiSsid, const char *wifiPassword, bool hotspotFallback) { diff --git a/ArduinoUtils.h b/ArduinoUtils.h index e00514f..f2676ed 100644 --- a/ArduinoUtils.h +++ b/ArduinoUtils.h @@ -1,4 +1,10 @@ #pragma once +#if defined(ARDUINO) +#include bool StartWifi(const char *wifiSsid, const char *wifiPassword, - bool hotspotFallback); \ No newline at end of file + bool hotspotFallback); + +void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION); + +#endif \ No newline at end of file diff --git a/Messages.cpp b/Messages.cpp index ab0e4c3..dab6b01 100644 --- a/Messages.cpp +++ b/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 diff --git a/Messages.h b/Messages.h index f0d5cd4..b34f247 100644 --- a/Messages.h +++ b/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 diff --git a/ClientMsg.cpp b/Messages/ClientMsg.cpp similarity index 94% rename from ClientMsg.cpp rename to Messages/ClientMsg.cpp index c749092..083cb89 100644 --- a/ClientMsg.cpp +++ b/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; diff --git a/ClientMsg.h b/Messages/ClientMsg.h similarity index 96% rename from ClientMsg.h rename to Messages/ClientMsg.h index 5115c7c..4d45566 100644 --- a/ClientMsg.h +++ b/Messages/ClientMsg.h @@ -16,6 +16,7 @@ public: ClientMsg(char networkId); ClientMsg(const char *buffer); + virtual ~ClientMsg(); virtual unsigned char Serialize(char *buffer) override; }; diff --git a/Messages/CustomMsg.h b/Messages/CustomMsg.h index ed7c02c..169562a 100644 --- a/Messages/CustomMsg.h +++ b/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; diff --git a/Messages/DestroyMsg.cpp b/Messages/DestroyMsg.cpp new file mode 100644 index 0000000..ecd1b1a --- /dev/null +++ b/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 diff --git a/Messages/DestroyMsg.h b/Messages/DestroyMsg.h new file mode 100644 index 0000000..4d83c17 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Messages/InvestigateMsg.cpp b/Messages/InvestigateMsg.cpp new file mode 100644 index 0000000..da449a9 --- /dev/null +++ b/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 diff --git a/Messages/InvestigateMsg.h b/Messages/InvestigateMsg.h new file mode 100644 index 0000000..21cf98d --- /dev/null +++ b/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; +}; diff --git a/ModelUrlMsg.cpp b/Messages/ModelUrlMsg.cpp similarity index 97% rename from ModelUrlMsg.cpp rename to Messages/ModelUrlMsg.cpp index 67add43..01cf874 100644 --- a/ModelUrlMsg.cpp +++ b/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; diff --git a/ModelUrlMsg.h b/Messages/ModelUrlMsg.h similarity index 95% rename from ModelUrlMsg.h rename to Messages/ModelUrlMsg.h index 780529e..0cce9f5 100644 --- a/ModelUrlMsg.h +++ b/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; }; diff --git a/NameMsg.cpp b/Messages/NameMsg.cpp similarity index 98% rename from NameMsg.cpp rename to Messages/NameMsg.cpp index 20de696..6c0d054 100644 --- a/NameMsg.cpp +++ b/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; diff --git a/NameMsg.h b/Messages/NameMsg.h similarity index 96% rename from NameMsg.h rename to Messages/NameMsg.h index 80ba56e..58ec135 100644 --- a/NameMsg.h +++ b/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; }; diff --git a/NetworkIdMsg.cpp b/Messages/NetworkIdMsg.cpp similarity index 94% rename from NetworkIdMsg.cpp rename to Messages/NetworkIdMsg.cpp index 69234ef..21ceabc 100644 --- a/NetworkIdMsg.cpp +++ b/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; diff --git a/NetworkIdMsg.h b/Messages/NetworkIdMsg.h similarity index 94% rename from NetworkIdMsg.h rename to Messages/NetworkIdMsg.h index 81cb4f0..21731bb 100644 --- a/NetworkIdMsg.h +++ b/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); diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp new file mode 100644 index 0000000..e20c107 --- /dev/null +++ b/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; +} diff --git a/Messages/PoseMsg.h b/Messages/PoseMsg.h new file mode 100644 index 0000000..a5facd9 --- /dev/null +++ b/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; +}; diff --git a/ThingMsg.cpp b/Messages/ThingMsg.cpp similarity index 97% rename from ThingMsg.cpp rename to Messages/ThingMsg.cpp index 8fa51f3..d0d5e2b 100644 --- a/ThingMsg.cpp +++ b/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; diff --git a/ThingMsg.h b/Messages/ThingMsg.h similarity index 96% rename from ThingMsg.h rename to Messages/ThingMsg.h index edda54e..4fcdf56 100644 --- a/ThingMsg.h +++ b/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; }; diff --git a/Participant.cpp b/Participant.cpp index 677bc14..a251df0 100644 --- a/Participant.cpp +++ b/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,12 +42,16 @@ 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, - const char *remoteIpAddress, - int remotePort) { +void Participant::begin() { + 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(this); thisWindows->Setup(localPort, remoteIpAddress, remotePort); @@ -57,6 +62,7 @@ void Passer::Control::Participant::SetupUDP(int localPort, UdpArduino *thisArduino = static_cast(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); - this->Send(msg); - delete msg; - msg = new NameMsg(this->networkId, thing); - this->Send(msg); - delete msg; - msg = new ModelUrlMsg(this->networkId, thing); - this->Send(msg); - delete msg; + ThingMsg *thingMsg = new ThingMsg(this->networkId, thing); + this->Send(thingMsg); + delete thingMsg; + NameMsg *nameMsg = new NameMsg(this->networkId, thing); + this->Send(nameMsg); + delete nameMsg; + ModelUrlMsg *modelMsg = new ModelUrlMsg(this->networkId, thing); + this->Send(modelMsg); + 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)]; - strcpy(thing->name, msg->name); + char *thingName = new char[strlen(msg->name)]; + strcpy(thingName, msg->name); + thing->name = thingName; std::cout << "thing name = " << thing->name << "\n"; } } diff --git a/Participant.h b/Participant.h index a2b9c14..977b960 100644 --- a/Participant.h +++ b/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 "NameMsg.h" -#include "NetworkIdMsg.h" -#include "ThingMsg.h" +#include "Messages/InvestigateMsg.h" +#include "Messages/ModelUrlMsg.h" +#include "Messages/NameMsg.h" +#include "Messages/NetworkIdMsg.h" +#include "Messages/PoseMsg.h" +#include "Messages/ThingMsg.h" #include @@ -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);