Support sending poses from Unity changes

LinearAlgebra/src/SwingTwist.cs

@@ -29,11 +29,18 @@ namespace LinearAlgebra {
         }
 
 #if UNITY_5_3_OR_NEWER
+        public static SwingTwist FromQuaternion(Quaternion q) {
+            // q.ToAngles(out float right, out float up, out float forward);
+            UnityEngine.Vector3 angles = q.eulerAngles;
+            SwingTwist r = new SwingTwist(angles.y, angles.x, angles.z);
+            return r;
+        }
+
         public Quaternion ToQuaternion() {
             Quaternion q = Quaternion.Euler(-this.swing.vertical,
                                    this.swing.horizontal,
                                    this.twist);
-                                   return q;
+            return q;
         }
 #endif        
     }

Unity/Thing.cs

@@ -88,6 +88,7 @@ namespace RoboidControl.Unity {
         /// </summary>
         protected virtual void FixedUpdate() {
             UpdateThing();
+            core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);
         }
 
         /// <summary>

src/Thing.cs

@@ -340,6 +340,7 @@ namespace RoboidControl {
                 if (_orientation != value) {
                     _orientation = value;
                     orientationUpdated = true;
+                    updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
                     //OnOrientationChanged?.Invoke();
                 }
             }