diff --git a/Messages/LowLevelMessages.cpp b/Messages/LowLevelMessages.cpp
index a186511..4526a04 100644
--- a/Messages/LowLevelMessages.cpp
+++ b/Messages/LowLevelMessages.cpp
@@ -5,6 +5,83 @@
 
 namespace RoboidControl {
 
+void LowLevelMessages::SendSpherical(char* buffer,
+                                     unsigned char* ix,
+                                     Spherical s) {
+  SendFloat16(buffer, ix, s.distance);
+  SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
+  SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
+}
+Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
+                                             unsigned char* startIndex) {
+  float distance = ReceiveFloat16(buffer, startIndex);
+
+  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
+  Angle horizontal = Angle::Radians(horizontal8.InRadians());
+
+  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
+  Angle vertical = Angle::Radians(vertical8.InRadians());
+
+  Spherical s = Spherical(distance, horizontal, vertical);
+  return s;
+}
+
+void LowLevelMessages::SendSwingTwist(char* buffer,
+                                      unsigned char* ix,
+                                      SwingTwist s) {
+  SendAngle8(buffer, ix, s.swing.horizontal.InDegrees());
+  SendAngle8(buffer, ix, s.swing.vertical.InDegrees());
+  SendAngle8(buffer, ix, s.twist.InDegrees());
+}
+
+SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer,
+                                               unsigned char* startIndex) {
+  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
+  Angle horizontal = Angle::Radians(horizontal8.InRadians());
+
+  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
+  Angle vertical = Angle::Radians(vertical8.InRadians());
+
+  Angle8 twist8 = ReceiveAngle8(buffer, startIndex);
+  Angle twist = Angle::Radians(twist8.InRadians());
+
+  SwingTwist s = SwingTwist(horizontal, vertical, twist);
+  return s;
+}
+
+void LowLevelMessages::SendQuat32(char* buffer,
+                                  unsigned char* ix,
+                                  SwingTwist 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;
+  }
+  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
+  // << "\n";
+  buffer[(*ix)++] = qx;
+  buffer[(*ix)++] = qy;
+  buffer[(*ix)++] = qz;
+  buffer[(*ix)++] = qw;
+}
+
+SwingTwist LowLevelMessages::ReceiveQuat32(const 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);
+  SwingTwist s = SwingTwist::FromQuaternion(q);
+  return s;
+}
+
 void LowLevelMessages::SendAngle8(char* buffer,
                                   unsigned char* ix,
                                   const float angle) {
@@ -42,58 +119,5 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
   return (float)f.toFloat();
 }
 
-void LowLevelMessages::SendSpherical(char* buffer,
-                                     unsigned char* ix,
-                                     Spherical s) {
-  SendFloat16(buffer, ix, s.distance);
-  SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
-  SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
-}
-Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
-                                             unsigned char* startIndex) {
-  float distance = ReceiveFloat16(buffer, startIndex);
-
-  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
-  Angle horizontal = Angle::Radians(horizontal8.InRadians());
-
-  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
-  Angle vertical = Angle::Radians(vertical8.InRadians());
-
-  Spherical s = Spherical(distance, horizontal, vertical);
-  return s;
-}
-
-void LowLevelMessages::SendQuat32(char* buffer,
-                                  unsigned char* ix,
-                                  SwingTwist 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;
-  }
-  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
-  // << "\n";
-  buffer[(*ix)++] = qx;
-  buffer[(*ix)++] = qy;
-  buffer[(*ix)++] = qz;
-  buffer[(*ix)++] = qw;
-}
-
-SwingTwist LowLevelMessages::ReceiveQuat32(const 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);
-  SwingTwist s = SwingTwist::FromQuaternion(q);
-  return s;
-}
 
 }  // namespace RoboidControl
\ No newline at end of file
diff --git a/Messages/LowLevelMessages.h b/Messages/LowLevelMessages.h
index 144e50c..c21b32d 100644
--- a/Messages/LowLevelMessages.h
+++ b/Messages/LowLevelMessages.h
@@ -11,6 +11,10 @@ class LowLevelMessages {
   static Spherical ReceiveSpherical(const char* buffer,
                                     unsigned char* startIndex);
 
+  static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s);
+  static SwingTwist ReceiveSwingTwist(const char* buffer,
+                                      unsigned char* startIndex);
+
   static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
   static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
 
diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp
index 79d2e4a..aca128e 100644
--- a/Messages/PoseMsg.cpp
+++ b/Messages/PoseMsg.cpp
@@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
   this->thingId = buffer[ix++];
   this->poseType = buffer[ix++];
   this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
-  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
+  this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
   // linearVelocity
   // angularVelocity
 }
@@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   if ((this->poseType & Pose_Position) != 0)
     LowLevelMessages::SendSpherical(buffer, &ix, this->position);
   if ((this->poseType & Pose_Orientation) != 0)
-    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
+    LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
   if ((this->poseType & Pose_LinearVelocity) != 0)
     LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
   if ((this->poseType & Pose_AngularVelocity) != 0)