Migrated to ControlCore

This commit is contained in:
Pascal Serrarens 2024-12-15 13:17:04 +01:00
parent b190b6830c
commit 821a522003
7 changed files with 316 additions and 54 deletions

View File

@ -1,7 +1,5 @@
#pragma once
#include "Participant.h"
namespace Passer::Control {
class CoreThing {
@ -31,3 +29,4 @@ protected:
};
} // namespace Passer::Control
using namespace Passer::Control;

View File

@ -7,6 +7,14 @@ void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
Angle8 packedAngle2 = Angle8::Degrees(angle);
buffer[(*ix)++] = packedAngle2.GetBinary();
}
Angle8 LowLevelMessages::ReceiveAngle8(const unsigned char *buffer,
unsigned char *startIndex) {
unsigned char binary = buffer[(*startIndex)++];
Angle8 angle = Angle8::Binary(binary);
return angle;
}
void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
float value) {
@ -16,14 +24,36 @@ void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
buffer[(*ix)++] = (binary >> 8) & 0xFF;
buffer[(*ix)++] = binary & 0xFF;
}
float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
unsigned char *startIndex) {
unsigned char ix = *startIndex;
unsigned short value = buffer[ix++] << 8 | buffer[ix++];
float16 f = float16();
f.setBinary(value);
void Passer::Control::LowLevelMessages::SendSpherical16(unsigned char *buffer,
unsigned char *ix,
Spherical16 s) {
*startIndex = ix;
return f.toDouble();
}
void LowLevelMessages::SendSpherical16(unsigned char *buffer, unsigned char *ix,
Spherical16 s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
Spherical16 LowLevelMessages::ReceiveSpherical16(const unsigned char *buffer,
unsigned char *startIndex) {
float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
Spherical16 s = Spherical16(distance, horizontal, vertical);
return s;
}
void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
unsigned char *ix,
@ -45,3 +75,14 @@ void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw;
}
SwingTwist16 LowLevelMessages::ReceiveQuat32(const unsigned char *buffer,
unsigned char *ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qw = buffer[(*ix)++] / 255.0F;
Quaternion q = Quaternion(qx, qy, qz, qw);
SwingTwist16 s = SwingTwist16::FromQuaternion(q);
return s;
}

View File

@ -7,12 +7,23 @@ class LowLevelMessages {
public:
static void SendAngle8(unsigned char *buffer, unsigned char *ix,
const float angle);
static Angle8 ReceiveAngle8(const unsigned char *buffer,
unsigned char *startIndex);
static void SendFloat16(unsigned char *buffer, unsigned char *ix,
float value);
static float ReceiveFloat16(const unsigned char *buffer,
unsigned char *startIndex);
static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
Spherical16 s);
static Spherical16 ReceiveSpherical16(const unsigned char *buffer,
unsigned char *startIndex);
static void SendQuat32(unsigned char *buffer, unsigned char *ix,
SwingTwist16 q);
static SwingTwist16 ReceiveQuat32(const unsigned char *buffer,
unsigned char *ix);
};
} // namespace Passer::Control

View File

@ -1,21 +1,81 @@
#include "Messages.h"
#include "LowLevelMessages.h"
#include "Participant.h"
#include "string.h"
#pragma region IMessage
IMessage::IMessage() {}
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }
bool IMessage::SendMsg(Participant *client, IMessage msg) {
// return SendMsg(client, client.buffer, );nameLength
return client->SendBuffer(msg.Serialize(client->buffer));
// void IMessage::Deserialize(unsigned char *buffer) {}
// bool IMessage::SendMsg(Participant *client, IMessage msg) {
// // return SendMsg(client, client.buffer, );nameLength
// return client->SendBuffer(msg.Serialize(client->buffer));
// }
unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
return nullptr;
}
bool IMessage::Publish(Participant *participant) {
return participant->PublishBuffer(Serialize(participant->buffer));
}
bool IMessage::Send(Participant *participant) {
return participant->SendBuffer(Serialize(participant->buffer));
}
// IMessage
#pragma endregion
#pragma region Client
ClientMsg::ClientMsg(unsigned char networkId) { this->networkId = networkId; }
unsigned char ClientMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
return ix;
}
// bool ClientMsg::Send(Participant *participant, unsigned char networkId) {
// ClientMsg msg = ClientMsg()
// }
// Client Msg
#pragma endregion
#pragma region Network Id
NetworkIdMsg::NetworkIdMsg(unsigned char *buffer) {
this->networkId = buffer[1];
}
// void NetworkIdMsg::Deserialize(unsigned char *buffer) {
// this->networkId = buffer[1];
// }
NetworkIdMsg NetworkIdMsg::Receive(unsigned char *buffer,
unsigned char bufferSize) {
NetworkIdMsg msg = NetworkIdMsg(buffer);
return msg;
}
// Network Id
#pragma endregion
#pragma region Investigate
InvestigateMsg::InvestigateMsg(unsigned 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;
@ -29,17 +89,25 @@ unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
return ix;
}
bool InvestigateMsg::Send(Participant *client, unsigned char networkId,
unsigned char thingId) {
InvestigateMsg msg = InvestigateMsg(networkId, thingId);
return SendMsg(client, msg);
}
// 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 Thing
ThingMsg::ThingMsg(const unsigned char *buffer) {
unsigned char ix = 1; // first byte is msg id
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->thingType = buffer[ix++];
this->parentId = buffer[ix++];
}
ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
unsigned char thingType, unsigned char parentId) {
this->networkId = networkId;
@ -58,12 +126,12 @@ unsigned char ThingMsg::Serialize(unsigned char *buffer) {
return ix;
}
bool ThingMsg::Send(Participant *client, unsigned char networkId,
unsigned char thingId, unsigned char thingType,
unsigned char parentId) {
ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
return SendMsg(client, msg);
}
// bool ThingMsg::Send(Participant *participant, unsigned char networkId,
// unsigned char thingId, unsigned char thingType,
// unsigned char parentId) {
// ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
// return msg.Send(participant);
// }
// Thing
#pragma endregion
@ -90,17 +158,17 @@ unsigned char NameMsg::Serialize(unsigned char *buffer) {
return ix;
}
bool NameMsg::Send(Participant *client, CoreThing *thing,
unsigned char nameLength) {
if (thing->name == nullptr)
return true; // nothing sent, but still a success!
// bool NameMsg::Send(Participant *participant, CoreThing *thing,
// unsigned char nameLength) {
// if (thing->name == nullptr)
// return true; // nothing sent, but still a success!
if (strlen(thing->name) == 0)
return true;
// if (strlen(thing->name) == 0)
// return true;
NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name, nameLength);
return SendMsg(client, msg);
}
// NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name,
// nameLength); return msg.Send(participant);
// }
// Name
#pragma endregion
@ -134,7 +202,6 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
#pragma region PoseMsg
#include <Arduino.h>
PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation) {
@ -144,6 +211,14 @@ PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
this->orientation = orientation;
this->poseType = poseType;
}
PoseMsg::PoseMsg(const unsigned 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(unsigned char *buffer) {
unsigned char ix = 0;
@ -152,11 +227,66 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) {
buffer[ix++] = this->thingId;
buffer[ix++] = this->poseType;
LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
printf("send spherical %f %f\n", this->position.distance,
this->position.direction.horizontal.InDegrees());
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
return ix;
}
// Pose
#pragma endregion
#pragma endregion
#pragma region CustomMsg
CustomMsg::CustomMsg(unsigned char *buffer) {
unsigned char ix;
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->dataSize = buffer[ix++];
this->data =
buffer + ix; // This is only valid because the code ensures the the msg
// lifetime is shorter than the buffer lifetime...
}
CustomMsg::CustomMsg(unsigned char networkId, unsigned char thingId,
unsigned char *data, unsigned char dataSize) {
this->networkId = networkId;
this->thingId = thingId;
this->dataSize = dataSize;
this->data = data;
}
unsigned char CustomMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
for (int dataIx = 0; dataIx < this->dataSize; dataIx++)
buffer[ix++] = this->data[dataIx];
return ix;
}
// void CustomMsg::Deserialize(unsigned char *buffer) {
// unsigned char ix;
// this->networkId = buffer[ix++];
// this->thingId = buffer[ix++];
// this->dataSize = buffer[ix++];
// this->data = buffer + ix; // challenging: point directly into the buffer!
// // this->data = new unsigned char[this->dataSize]; // memory leak!
// // for (unsigned char dataIx = 0; dataIx < this->dataSize; dataIx++)
// // this->data[dataIx] = buffer[ix++];
// }
// bool CustomMsg::Send(Participant *participant, unsigned char networkId,
// unsigned char thingId, unsigned char *data,
// unsigned char dataSize) {
// CustomMsg msg = CustomMsg(networkId, thingId, data, dataSize);
// return msg.Send(participant);
// }
CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
CustomMsg msg = CustomMsg(buffer);
return msg;
}
// CustomMsg
#pragma endregion

View File

@ -1,17 +1,43 @@
#pragma once
#include "../LinearAlgebra/Spherical.h"
#include "../LinearAlgebra/SwingTwist.h"
#include "../float16/float16.h"
#include "CoreThing.h"
#include "Participant.h"
namespace Passer::Control {
class Participant;
class IMessage {
public:
IMessage();
virtual unsigned char Serialize(unsigned char *buffer);
static bool SendMsg(Participant *client, IMessage msg);
static unsigned char *ReceiveMsg(unsigned char packetSize);
bool Publish(Participant *participant);
bool Send(Participant *participant);
};
class ClientMsg : public IMessage {
public:
static const unsigned char id = 0xA0;
unsigned char networkId;
ClientMsg(unsigned char networkId);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class NetworkIdMsg : public IMessage {
public:
static const unsigned char id = 0xA1;
static const unsigned char length = 2;
unsigned char networkId;
NetworkIdMsg(unsigned char *buffer);
static NetworkIdMsg Receive(unsigned char *buffer, unsigned char bufferSize);
};
class InvestigateMsg : public IMessage {
@ -21,12 +47,10 @@ public:
unsigned char networkId;
unsigned char thingId;
InvestigateMsg(unsigned char *buffer);
InvestigateMsg(unsigned char networkId, unsigned char thingId);
virtual unsigned char Serialize(unsigned char *buffer) override;
static bool Send(Participant *client, unsigned char networkId,
unsigned char thingId);
};
class ThingMsg : public IMessage {
@ -38,14 +62,11 @@ public:
unsigned char thingType;
unsigned char parentId;
ThingMsg(const unsigned char *buffer);
ThingMsg(unsigned char networkId, unsigned char thingId,
unsigned char thingType, unsigned char parentId);
virtual unsigned char Serialize(unsigned char *buffer) override;
static bool Send(Participant *client, unsigned char networkId,
unsigned char thingId, unsigned char thingType,
unsigned char parentId);
};
class NameMsg : public IMessage {
@ -61,9 +82,6 @@ public:
unsigned char nameLength);
virtual unsigned char Serialize(unsigned char *buffer) override;
static bool Send(Participant *client, CoreThing *thing,
unsigned char nameLength);
};
class ModelUrlMsg : public IMessage {
@ -92,8 +110,10 @@ public:
unsigned char thingId;
unsigned char poseType;
const unsigned char Pose_Position = 0x01;
const unsigned char Pose_Orientation = 0x02;
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;
@ -101,9 +121,31 @@ public:
PoseMsg(unsigned char networkId, unsigned char thingId,
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation);
PoseMsg(const unsigned char *buffer);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class CustomMsg : public IMessage {
public:
static const unsigned char id = 0xB1;
static const unsigned length = 3;
unsigned char networkId;
unsigned char thingId;
unsigned char dataSize;
unsigned char *data;
CustomMsg(unsigned char *buffer);
CustomMsg(unsigned char networkId, unsigned char thingId, unsigned char *data,
unsigned char dataSize);
virtual unsigned char Serialize(unsigned char *buffer) override;
static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
};
} // namespace Passer::Control
using namespace Passer::Control;

View File

@ -1,10 +1,40 @@
#include "Participant.h"
// bool Passer::Control::ControlClient::SendMsg(unsigned char *buffer,
// unsigned char bufferSize) {
// return false;
// }
bool Participant::SendBuffer(unsigned char bufferSize) { return false; }
bool Participant::PublishBuffer(unsigned char bufferSize) { return false; }
void Participant::ReceiveData(unsigned char bufferSize) {
unsigned char msgId = this->buffer[0];
switch (msgId) {
case NetworkIdMsg::id: {
// NetworkIdMsg msg = NetworkIdMsg::Receive(this->buffer, bufferSize);
NetworkIdMsg msg = NetworkIdMsg(this->buffer);
ProcessNetworkIdMsg(msg);
} break;
case InvestigateMsg::id: {
InvestigateMsg msg = InvestigateMsg(this->buffer);
ProcessInvestigateMsg(msg);
} break;
case ThingMsg::id: {
ThingMsg msg = ThingMsg(this->buffer);
ProcessThingMsg(msg);
} break;
case PoseMsg::id: {
PoseMsg msg = PoseMsg(this->buffer);
ProcessPoseMsg(msg);
} break;
case CustomMsg::id: {
CustomMsg msg = CustomMsg(this->buffer);
ProcessCustomMsg(msg);
} break;
};
}
void Participant::ProcessNetworkIdMsg(NetworkIdMsg msg) {}
void Participant::ProcessInvestigateMsg(InvestigateMsg msg) {}
void Participant::ProcessThingMsg(ThingMsg msg) {}
void Participant::ProcessCustomMsg(CustomMsg msg) {}

View File

@ -1,15 +1,24 @@
#pragma once
#include "Messages.h"
namespace Passer::Control {
class Participant {
public:
// unsigned char *buffer;
// bool SendMsg(unsigned char *buffer, unsigned char bufferSize);
unsigned char buffer[1024];
virtual bool SendBuffer(unsigned char bufferSize);
virtual bool PublishBuffer(unsigned char bufferSize);
void ReceiveData(unsigned char bufferSize);
protected:
virtual void ProcessNetworkIdMsg(NetworkIdMsg msg);
virtual void ProcessInvestigateMsg(InvestigateMsg msg);
virtual void ProcessThingMsg(ThingMsg msg);
virtual void ProcessPoseMsg(PoseMsg msg);
virtual void ProcessCustomMsg(CustomMsg msg);
};
} // namespace Passer::Control