Improved accuracy, fixed field order in sending sperical

CoreThing.cpp

@@ -8,31 +8,31 @@ CoreThing::CoreThing(
   this->type = thingType;
   this->networkId = networkId;
   this->Init();
-  // CoreThings::Add(this);
+  CoreThing::Add(this);
 }
 
 void CoreThing::Init() {}
 
-// CoreThing *CoreThings::allThings[256] = {nullptr};
+CoreThing *CoreThing::allThings[256] = {nullptr};
 
-// CoreThing *CoreThings::Get(unsigned char networkId, unsigned char thingId) {
-//   for (unsigned char ix = 0; ix < 256; ix++) {
-//     CoreThing *thing = allThings[ix];
-//     if (thing == nullptr)
-//       continue;
-//     if (thing->networkId == networkId && thing->id == thingId)
-//       return thing;
-//   }
-//   return nullptr;
-// }
+CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
+  for (unsigned char ix = 0; ix < 256; ix++) {
+    CoreThing *thing = allThings[ix];
+    if (thing == nullptr)
+      continue;
+    if (thing->networkId == networkId && thing->id == thingId)
+      return thing;
+  }
+  return nullptr;
+}
 
-// bool CoreThings::Add(CoreThing *newThing) {
-//   for (unsigned char ix = 0; ix < 256; ix++) {
-//     CoreThing *thing = allThings[ix];
-//     if (thing == nullptr) {
-//       allThings[ix] = newThing;
-//       return true;
-//     }
-//   }
-//   return false;
-// }
+bool CoreThing::Add(CoreThing *newThing) {
+  for (unsigned char ix = 0; ix < 256; ix++) {
+    CoreThing *thing = allThings[ix];
+    if (thing == nullptr) {
+      allThings[ix] = newThing;
+      return true;
+    }
+  }
+  return false;
+}

CoreThing.h

@@ -15,27 +15,19 @@ public:
   const char *modelUrl;
   // protected Sensor sensor;
 
+  static CoreThing *allThings[];
+
 public:
   CoreThing(
       // Participant *client,
       unsigned char networkId, unsigned char thingId,
       unsigned char thingType = 0);
 
+  static CoreThing *Get(unsigned char networkId, unsigned char thingId);
+  static bool Add(CoreThing *thing);
+
 protected:
   virtual void Init();
-
-  // Things
 };
 
-// class CoreThings {
-//   friend class CoreThing;
-
-// public:
-//   static CoreThing *allThings[];
-
-// public:
-//   static CoreThing *Get(unsigned char networkId, unsigned char thingId);
-//   static bool Add(CoreThing *thing);
-// };
-
 } // namespace Passer::Control

LowLevelMessages.cpp

@@ -2,6 +2,12 @@
 
 #include "../float16/float16.h"
 
+void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
+                                  const float angle) {
+  Angle8 packedAngle2 = Angle8::Degrees(angle);
+  buffer[(*ix)++] = packedAngle2.GetBinary();
+}
+
 void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
                                    float value) {
   float16 value16 = float16(value);
@@ -9,4 +15,33 @@ void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
 
   buffer[(*ix)++] = (binary >> 8) & 0xFF;
   buffer[(*ix)++] = binary & 0xFF;
-}
+}
+
+void Passer::Control::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());
+}
+
+void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
+                                                   unsigned char *ix,
+                                                   SwingTwist16 rotation) {
+  Quaternion q = rotation.ToQuaternion();
+  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[(*ix)++] = qx;
+  buffer[(*ix)++] = qy;
+  buffer[(*ix)++] = qz;
+  buffer[(*ix)++] = qw;
+}

LowLevelMessages.h

@@ -1,11 +1,18 @@
 #include "../LinearAlgebra/Spherical.h"
+#include "../LinearAlgebra/SwingTwist.h"
 
 namespace Passer::Control {
 
 class LowLevelMessages {
 public:
+  static void SendAngle8(unsigned char *buffer, unsigned char *ix,
+                         const float angle);
   static void SendFloat16(unsigned char *buffer, unsigned char *ix,
                           float value);
+  static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
+                              Spherical16 s);
+  static void SendQuat32(unsigned char *buffer, unsigned char *ix,
+                         SwingTwist16 q);
 };
 
 } // namespace Passer::Control

Messages.cpp

@@ -130,4 +130,33 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
 }
 
 // Model Url
+#pragma endregion
+
+#pragma region PoseMsg
+
+#include <Arduino.h>
+PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
+                 unsigned char poseType, Spherical16 position,
+                 SwingTwist16 orientation) {
+  this->networkId = networkId;
+  this->thingId = thingId;
+  this->position = position;
+  this->orientation = orientation;
+  this->poseType = poseType;
+}
+
+unsigned char PoseMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = PoseMsg::id;
+  buffer[ix++] = this->networkId;
+  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

Messages.h

@@ -1,4 +1,6 @@
 #pragma once
+#include "../LinearAlgebra/Spherical.h"
+#include "../LinearAlgebra/SwingTwist.h"
 #include "../float16/float16.h"
 #include "CoreThing.h"
 #include "Participant.h"
@@ -67,8 +69,10 @@ public:
 class ModelUrlMsg : public IMessage {
 public:
   static const unsigned char id = 0x90;
+
   unsigned char networkId;
   unsigned char thingId;
+
   float scale;
   unsigned char urlLength;
   const char *url;
@@ -79,5 +83,27 @@ public:
   virtual unsigned char Serialize(unsigned 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;
+  const unsigned char Pose_Position = 0x01;
+  const unsigned char Pose_Orientation = 0x02;
+
+  Spherical16 position;
+  SwingTwist16 orientation;
+
+  PoseMsg(unsigned char networkId, unsigned char thingId,
+          unsigned char poseType, Spherical16 position,
+          SwingTwist16 orientation);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
 } // namespace Passer::Control
 using namespace Passer::Control;