Separated out low-level messaging

Messages.cpp

@@ -2,17 +2,19 @@
 
 #include "float16/float16.h"
 
-Angle8 Messages::ReceiveAngle8(unsigned char *data, unsigned char *startIndex) {
+Angle8 Messages::ReceiveAngle8(unsigned char *buffer,
-  unsigned char binary = data[(*startIndex)++];
+                               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];
+//   }
+// }

Messages.h

@@ -7,10 +7,37 @@ namespace RoboidControl {
 
 class Messages {
 public:
-  static Angle8 ReceiveAngle8(unsigned char *data, unsigned char *startIndex);
+  static Angle8 ReceiveAngle8(unsigned char *buffer, unsigned char *startIndex);
-  static float ReceiveFloat16(unsigned char *data, unsigned char *startIndex);
+  static float ReceiveFloat16(unsigned char *buffer, unsigned char *startIndex);
-  static Spherical16 ReceiveSpherical16(unsigned char *data,
+  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

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);
+  Messages::SendSpherical16(buffer, &ix, s);
-  SendFloat16(buffer, &ix, thing->modelScale);
+  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);
+    Messages::SendSpherical16(buffer, &ix, thing->position);
-    SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
+    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);
+  Messages::SendSpherical16(buffer, &ix, worldPosition);
-  SendSwingTwist(buffer, &ix, worldOrientation);
+  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) {}

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);