Separated out low-level messaging
This commit is contained in:
Pascal Serrarens
parent
fb5314019c
commit
3132912afc
125
Messages.cpp
125
Messages.cpp
@ -2,17 +2,19 @@
|
||||
|
||||
#include "float16/float16.h"
|
||||
|
||||
Angle8 Messages::ReceiveAngle8(unsigned char *data, unsigned char *startIndex) {
|
||||
unsigned char binary = data[(*startIndex)++];
|
||||
Angle8 Messages::ReceiveAngle8(unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
unsigned char binary = buffer[(*startIndex)++];
|
||||
|
||||
Angle8 angle = Angle8::Binary(binary);
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
float Messages::ReceiveFloat16(unsigned char *data, unsigned char *startIndex) {
|
||||
float Messages::ReceiveFloat16(unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
unsigned char ix = *startIndex;
|
||||
unsigned short value = data[ix++] << 8 | data[ix];
|
||||
unsigned short value = buffer[ix++] << 8 | buffer[ix];
|
||||
float16 f = float16();
|
||||
f.setBinary(value);
|
||||
|
||||
@ -20,16 +22,123 @@ float Messages::ReceiveFloat16(unsigned char *data, unsigned char *startIndex) {
|
||||
return f.toDouble();
|
||||
}
|
||||
|
||||
Spherical16 Messages::ReceiveSpherical16(unsigned char *data,
|
||||
Spherical16 Messages::ReceiveSpherical16(unsigned char *buffer,
|
||||
unsigned char *startIndex) {
|
||||
float distance = ReceiveFloat16(data, startIndex);
|
||||
float distance = ReceiveFloat16(buffer, startIndex);
|
||||
|
||||
Angle8 horizontal8 = ReceiveAngle8(data, startIndex);
|
||||
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 255);
|
||||
|
||||
Angle8 vertical8 = ReceiveAngle8(data, startIndex);
|
||||
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 255);
|
||||
|
||||
Spherical16 s = Spherical16(distance, horizontal, vertical);
|
||||
return s;
|
||||
}
|
||||
|
||||
void Messages::SendVector3(unsigned char *buffer, unsigned char *startIndex,
|
||||
const Vector3 v) {
|
||||
SendSingle100(buffer, *startIndex, v.Right());
|
||||
(*startIndex) += 4;
|
||||
SendSingle100(buffer, *startIndex, v.Up());
|
||||
(*startIndex) += 4;
|
||||
SendSingle100(buffer, *startIndex, v.Forward());
|
||||
(*startIndex) += 4;
|
||||
}
|
||||
|
||||
void Messages::SendQuaternion(unsigned char *buffer, const int startIndex,
|
||||
const Quaternion q) {
|
||||
Vector3 angles = Quaternion::ToAngles(q);
|
||||
int ix = startIndex;
|
||||
SendAngle8(buffer, ix++, angles.Right());
|
||||
SendAngle8(buffer, ix++, angles.Up());
|
||||
SendAngle8(buffer, ix++, angles.Forward());
|
||||
}
|
||||
|
||||
void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex,
|
||||
Polar p) {
|
||||
SendAngle8(buffer, *startIndex, (const float)p.angle.InDegrees());
|
||||
SendSingle100(buffer, (*startIndex) + 1, p.distance);
|
||||
}
|
||||
|
||||
void Messages::SendSpherical16(unsigned char *buffer, unsigned char *startIndex,
|
||||
Spherical16 s) {
|
||||
SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees());
|
||||
SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees());
|
||||
SendFloat16(buffer, startIndex, s.distance);
|
||||
}
|
||||
|
||||
void Messages::SendSwingTwist(unsigned char *buffer, unsigned char *ix,
|
||||
const SwingTwist16 r) {
|
||||
Quaternion q = r.ToQuaternion();
|
||||
SendQuat32(buffer, ix, q);
|
||||
}
|
||||
|
||||
void Messages::SendQuat32(unsigned char *buffer, unsigned char *startIndex,
|
||||
const Quaternion q) {
|
||||
unsigned char qx = (char)(q.x * 127 + 128);
|
||||
unsigned char qy = (char)(q.y * 127 + 128);
|
||||
unsigned char qz = (char)(q.z * 127 + 128);
|
||||
unsigned char qw = (char)(q.w * 255);
|
||||
if (q.w < 0) {
|
||||
qx = -qx;
|
||||
qy = -qy;
|
||||
qz = -qz;
|
||||
qw = -qw;
|
||||
}
|
||||
// Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
|
||||
buffer[(*startIndex)++] = qx;
|
||||
buffer[(*startIndex)++] = qy;
|
||||
buffer[(*startIndex)++] = qz;
|
||||
buffer[(*startIndex)++] = qw;
|
||||
}
|
||||
|
||||
void Messages::SendAngle8(unsigned char *buffer, unsigned int startIndex,
|
||||
const float angle) {
|
||||
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
||||
buffer[startIndex] = packedAngle2.GetBinary();
|
||||
}
|
||||
|
||||
// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
|
||||
// const float angle) {
|
||||
// AngleUsing<signed short> packedAngle = AngleUsing<signed short>(angle);
|
||||
// signed short value = packedAngle.GetValue();
|
||||
// data[startIndex] = value >> 8;
|
||||
// data[startIndex + 1] = value & 0xFF;
|
||||
// }
|
||||
|
||||
// void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
|
||||
// const float angle) {
|
||||
// AngleUsing<signed long> packedAngle = AngleUsing<signed long>(angle);
|
||||
// unsigned long value = packedAngle.GetValue();
|
||||
// data[startIndex] = value >> 24 & 0xFF;
|
||||
// data[startIndex + 1] = value >> 16 & 0xFF;
|
||||
// data[startIndex + 2] = value >> 8 & 0xFF;
|
||||
// data[startIndex + 3] = value & 0xFF;
|
||||
// // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
|
||||
// // data[startIndex + 1], data[startIndex + 2],
|
||||
// // data[startIndex + 3]);
|
||||
// }
|
||||
|
||||
// void Messages::SendSingle100(unsigned char *data, unsigned int startIndex,
|
||||
// float value) {
|
||||
// // Sends a float with truncated 2 decimal precision
|
||||
// Int32 intValue = value * 100;
|
||||
// SendInt32(data, startIndex, intValue);
|
||||
// }
|
||||
|
||||
void Messages::SendFloat16(unsigned char *data, unsigned char *startIndex,
|
||||
float value) {
|
||||
float16 value16 = float16(value);
|
||||
short binary = value16.getBinary();
|
||||
|
||||
data[(*startIndex)++] = (binary >> 8) & 0xFF;
|
||||
data[(*startIndex)++] = binary & 0xFF;
|
||||
}
|
||||
|
||||
// void Messages::SendInt32(unsigned char *data, unsigned int startIndex,
|
||||
// Int32 value) {
|
||||
// for (unsigned char ix = 0; ix < 4; ix++) {
|
||||
// data[startIndex++] = ((unsigned char *)&value)[ix];
|
||||
// }
|
||||
// }
|
||||
|
||||
33
Messages.h
33
Messages.h
@ -7,10 +7,37 @@ namespace RoboidControl {
|
||||
|
||||
class Messages {
|
||||
public:
|
||||
static Angle8 ReceiveAngle8(unsigned char *data, unsigned char *startIndex);
|
||||
static float ReceiveFloat16(unsigned char *data, unsigned char *startIndex);
|
||||
static Spherical16 ReceiveSpherical16(unsigned char *data,
|
||||
static Angle8 ReceiveAngle8(unsigned char *buffer, unsigned char *startIndex);
|
||||
static float ReceiveFloat16(unsigned char *buffer, unsigned char *startIndex);
|
||||
static Spherical16 ReceiveSpherical16(unsigned char *buffer,
|
||||
unsigned char *startIndex);
|
||||
|
||||
static void SendSingle100(unsigned char *buffer, unsigned int startIndex,
|
||||
float value);
|
||||
static void SendFloat16(unsigned char *buffer, unsigned char *startIndex,
|
||||
float value);
|
||||
|
||||
// void SendInt32(unsigned char *buffer, unsigned int startIndex, Int32
|
||||
// value);
|
||||
static void SendAngle8(unsigned char *buffer, unsigned int startIndex,
|
||||
const float value);
|
||||
// void SendAngle16(unsigned char *buffer, unsigned int startIndex,
|
||||
// const float value);
|
||||
// void SendAngle32(unsigned char *buffer, unsigned int startIndex,
|
||||
// const float value);
|
||||
static void SendVector3(unsigned char *buffer, unsigned char *startIndex,
|
||||
const Vector3 v);
|
||||
static void SendQuaternion(unsigned char *buffer, const int startIndex,
|
||||
const Quaternion q);
|
||||
|
||||
static void SendPolar(unsigned char *buffer, unsigned char *startIndex,
|
||||
Polar p);
|
||||
static void SendSpherical16(unsigned char *buffer, unsigned char *startIndex,
|
||||
Spherical16 s);
|
||||
static void SendSwingTwist(unsigned char *buffer, unsigned char *startIndex,
|
||||
const SwingTwist16 r);
|
||||
static void SendQuat32(unsigned char *buffer, unsigned char *startIndex,
|
||||
const Quaternion q);
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
|
||||
128
NetworkSync.cpp
128
NetworkSync.cpp
@ -1,4 +1,5 @@
|
||||
#include "NetworkSync.h"
|
||||
#include "Messages.h"
|
||||
|
||||
// #define RC_DEBUG 1
|
||||
|
||||
@ -150,8 +151,8 @@ void NetworkSync::SendModel(Thing *thing) {
|
||||
buffer[ix++] = ModelMsg;
|
||||
buffer[ix++] = thing->id; // objectId
|
||||
Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
|
||||
SendSpherical16(buffer, &ix, s);
|
||||
SendFloat16(buffer, &ix, thing->modelScale);
|
||||
Messages::SendSpherical16(buffer, &ix, s);
|
||||
Messages::SendFloat16(buffer, &ix, thing->modelScale);
|
||||
|
||||
buffer[ix++] = len;
|
||||
for (int urlIx = 0; urlIx < len; urlIx++)
|
||||
@ -190,8 +191,8 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
|
||||
if (thing->orientationUpdated)
|
||||
pattern |= Pose_Orientation;
|
||||
buffer[ix++] = pattern;
|
||||
SendSpherical16(buffer, &ix, thing->position);
|
||||
SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
|
||||
Messages::SendSpherical16(buffer, &ix, thing->position);
|
||||
Messages::SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
|
||||
SendBuffer(ix);
|
||||
|
||||
thing->positionUpdated = false;
|
||||
@ -226,7 +227,7 @@ void NetworkSync::PublishState(Sensor *sensor) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = StateMsg;
|
||||
buffer[ix++] = sensor->type;
|
||||
SendFloat16(buffer, &ix, *value);
|
||||
Messages::SendFloat16(buffer, &ix, *value);
|
||||
PublishBuffer(ix);
|
||||
}
|
||||
|
||||
@ -268,7 +269,7 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
|
||||
if (thing == nullptr || thing->updated == false || thing->networkId != 0x00) {
|
||||
return;
|
||||
}
|
||||
printf("publish tracked\n");
|
||||
// printf("publish tracked\n");
|
||||
/*
|
||||
Spherical16 roboidPosition = roboid->GetPosition();
|
||||
SwingTwist16 roboidOrientation = roboid->GetOrientation();
|
||||
@ -301,13 +302,13 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
|
||||
buffer[ix++] = PoseMsg; // Position2DMsg;
|
||||
buffer[ix++] = thing->id; // objectId;
|
||||
buffer[ix++] = Pose_Position | Pose_Orientation;
|
||||
SendSpherical16(buffer, &ix, worldPosition);
|
||||
SendSwingTwist(buffer, &ix, worldOrientation);
|
||||
Messages::SendSpherical16(buffer, &ix, worldPosition);
|
||||
Messages::SendSwingTwist(buffer, &ix, worldOrientation);
|
||||
SendBuffer(ix);
|
||||
|
||||
// #if RC_DEBUG
|
||||
#if RC_DEBUG
|
||||
printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], thing->type);
|
||||
// #endif
|
||||
#endif
|
||||
|
||||
thing->updated = false;
|
||||
}
|
||||
@ -362,113 +363,6 @@ void NetworkSync::SendInt(const int x) {
|
||||
|
||||
// Low-level functions
|
||||
|
||||
void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
|
||||
const Vector3 v) {
|
||||
SendSingle100(data, *startIndex, v.Right());
|
||||
(*startIndex) += 4;
|
||||
SendSingle100(data, *startIndex, v.Up());
|
||||
(*startIndex) += 4;
|
||||
SendSingle100(data, *startIndex, v.Forward());
|
||||
(*startIndex) += 4;
|
||||
}
|
||||
|
||||
void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
|
||||
const Quaternion q) {
|
||||
Vector3 angles = Quaternion::ToAngles(q);
|
||||
int ix = startIndex;
|
||||
SendAngle8(data, ix++, angles.Right());
|
||||
SendAngle8(data, ix++, angles.Up());
|
||||
SendAngle8(data, ix++, angles.Forward());
|
||||
}
|
||||
|
||||
void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
|
||||
Polar p) {
|
||||
SendAngle8(data, *startIndex, (const float)p.angle.InDegrees());
|
||||
SendSingle100(data, (*startIndex) + 1, p.distance);
|
||||
}
|
||||
|
||||
void NetworkSync::SendSpherical16(unsigned char *data,
|
||||
unsigned char *startIndex, Spherical16 s) {
|
||||
SendAngle8(data, (*startIndex)++, s.direction.horizontal.InDegrees());
|
||||
SendAngle8(data, (*startIndex)++, s.direction.vertical.InDegrees());
|
||||
SendFloat16(data, startIndex, s.distance);
|
||||
}
|
||||
|
||||
void NetworkSync::SendSwingTwist(unsigned char *data, unsigned char *ix,
|
||||
const SwingTwist16 r) {
|
||||
Quaternion q = r.ToQuaternion();
|
||||
SendQuat32(buffer, ix, q);
|
||||
}
|
||||
|
||||
void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex,
|
||||
const Quaternion q) {
|
||||
unsigned char qx = (char)(q.x * 127 + 128);
|
||||
unsigned char qy = (char)(q.y * 127 + 128);
|
||||
unsigned char qz = (char)(q.z * 127 + 128);
|
||||
unsigned char qw = (char)(q.w * 255);
|
||||
if (q.w < 0) {
|
||||
qx = -qx;
|
||||
qy = -qy;
|
||||
qz = -qz;
|
||||
qw = -qw;
|
||||
}
|
||||
// Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
|
||||
data[(*startIndex)++] = qx;
|
||||
data[(*startIndex)++] = qy;
|
||||
data[(*startIndex)++] = qz;
|
||||
data[(*startIndex)++] = qw;
|
||||
}
|
||||
|
||||
void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
|
||||
const float angle) {
|
||||
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
||||
data[startIndex] = packedAngle2.GetBinary();
|
||||
}
|
||||
|
||||
// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
|
||||
// const float angle) {
|
||||
// AngleUsing<signed short> packedAngle = AngleUsing<signed short>(angle);
|
||||
// signed short value = packedAngle.GetValue();
|
||||
// data[startIndex] = value >> 8;
|
||||
// data[startIndex + 1] = value & 0xFF;
|
||||
// }
|
||||
|
||||
// void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
|
||||
// const float angle) {
|
||||
// AngleUsing<signed long> packedAngle = AngleUsing<signed long>(angle);
|
||||
// unsigned long value = packedAngle.GetValue();
|
||||
// data[startIndex] = value >> 24 & 0xFF;
|
||||
// data[startIndex + 1] = value >> 16 & 0xFF;
|
||||
// data[startIndex + 2] = value >> 8 & 0xFF;
|
||||
// data[startIndex + 3] = value & 0xFF;
|
||||
// // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
|
||||
// // data[startIndex + 1], data[startIndex + 2],
|
||||
// // data[startIndex + 3]);
|
||||
// }
|
||||
|
||||
void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
|
||||
float value) {
|
||||
// Sends a float with truncated 2 decimal precision
|
||||
Int32 intValue = value * 100;
|
||||
SendInt32(data, startIndex, intValue);
|
||||
}
|
||||
|
||||
void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
|
||||
float value) {
|
||||
float16 value16 = float16(value);
|
||||
short binary = value16.getBinary();
|
||||
|
||||
data[(*startIndex)++] = (binary >> 8) & 0xFF;
|
||||
data[(*startIndex)++] = binary & 0xFF;
|
||||
}
|
||||
|
||||
void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
|
||||
Int32 value) {
|
||||
for (unsigned char ix = 0; ix < 4; ix++) {
|
||||
data[startIndex++] = ((unsigned char *)&value)[ix];
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkSync::SendBuffer(unsigned char bufferSize) {}
|
||||
|
||||
void NetworkSync::PublishBuffer(unsigned char bufferSize) {}
|
||||
@ -95,29 +95,6 @@ protected:
|
||||
// UInt8 parentId,
|
||||
// InterestingThing* object);
|
||||
|
||||
void SendSingle100(unsigned char *data, unsigned int startIndex, float value);
|
||||
void SendFloat16(unsigned char *data, unsigned char *startIndex, float value);
|
||||
|
||||
void SendInt32(unsigned char *data, unsigned int startIndex, Int32 value);
|
||||
void SendAngle8(unsigned char *data, unsigned int startIndex,
|
||||
const float value);
|
||||
// void SendAngle16(unsigned char *data, unsigned int startIndex,
|
||||
// const float value);
|
||||
// void SendAngle32(unsigned char *data, unsigned int startIndex,
|
||||
// const float value);
|
||||
void SendVector3(unsigned char *data, unsigned char *startIndex,
|
||||
const Vector3 v);
|
||||
void SendQuaternion(unsigned char *data, const int startIndex,
|
||||
const Quaternion q);
|
||||
|
||||
void SendPolar(unsigned char *data, unsigned char *startIndex, Polar p);
|
||||
void SendSpherical16(unsigned char *data, unsigned char *startIndex,
|
||||
Spherical16 s);
|
||||
void SendSwingTwist(unsigned char *data, unsigned char *startIndex,
|
||||
const SwingTwist16 r);
|
||||
void SendQuat32(unsigned char *data, unsigned char *startIndex,
|
||||
const Quaternion q);
|
||||
|
||||
unsigned char buffer[256];
|
||||
virtual void SendBuffer(unsigned char bufferSize);
|
||||
virtual void PublishBuffer(unsigned char bufferSize);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user