From 3132912afcd39b12732316c8eb6c5e37c44d053e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 2 Dec 2024 09:16:46 +0100 Subject: [PATCH] Separated out low-level messaging --- Messages.cpp | 127 +++++++++++++++++++++++++++++++++++++++++++---- Messages.h | 33 +++++++++++-- NetworkSync.cpp | 128 +++++------------------------------------------- NetworkSync.h | 23 --------- 4 files changed, 159 insertions(+), 152 deletions(-) diff --git a/Messages.cpp b/Messages.cpp index 7afd6ba..93f3f23 100644 --- a/Messages.cpp +++ b/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; -} \ No newline at end of file +} + +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 packedAngle = AngleUsing(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 packedAngle = AngleUsing(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]; +// } +// } diff --git a/Messages.h b/Messages.h index c0a3555..c9d9020 100644 --- a/Messages.h +++ b/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 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index be73a27..111d36e 100644 --- a/NetworkSync.cpp +++ b/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 packedAngle = AngleUsing(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 packedAngle = AngleUsing(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) {} \ No newline at end of file diff --git a/NetworkSync.h b/NetworkSync.h index 5a961c3..573e590 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -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);