Added conversion from quat to euler for rotation difference and added it to messaging

osc32bt/osc32bt.ino

@@ -2,10 +2,15 @@
 
 #include "Wire.h"
 #include "MPU6050_6Axis_MotionApps20.h"
-
+#include "BasicLinearAlgebra.h"
 #include "BluetoothSerial.h"
 #include <SLIPEncodedSerial.h>
 #include "SLIPEncodedBluetoothSerial.h"
+#include <BasicLinearAlgebra.h>
+#include "math.h"
+
+using namespace BLA;
+
 
 #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
 #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
@@ -200,6 +205,22 @@ void loop() {
     SLIPSerial.endPacket();
 
     quaternionDiffMessage.empty();
+
+    Matrix<3> eulerDiffVector = eulerFromQuaternion(diff);
+
+    eulerDiffMessage.add(eulerDiffVector(0));
+    eulerDiffMessage.add(eulerDiffVector(1));
+    eulerDiffMessage.add(eulerDiffVector(2));
+    
+    SLIPBTSerial.beginPacket();
+    eulerDiffMessage.send(SLIPBTSerial);
+    SLIPBTSerial.endPacket();
+    
+    SLIPSerial.beginPacket();
+    eulerDiffMessage.send(SLIPSerial);
+    SLIPSerial.endPacket();
+
+    eulerDiffMessage.empty();
     
 #endif
 
@@ -263,7 +284,6 @@ void loop() {
     msg.add(AcZ);
     msg.add(elapsedTime);
 
-    
     SLIPSerial.beginPacket();
     msg.send(SLIPSerial);
     SLIPSerial.endPacket();
@@ -291,3 +311,35 @@ void loop() {
     kmsg.empty();
   }
 }
+
+
+BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
+  float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
+  float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
+  float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
+  float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
+
+  BLA::Matrix<4,4> rotationMatrix = {
+    1 - (yy + zz), xy + wz, xz - wy, 0,
+    xy - wz,  1 - ( xx + zz ), yz + wx, 0,
+    xz + wy,  yz - wx, 1 - ( xx + yy ), 0,
+    0,  0,  0,  1
+  };
+
+//TODO: test whether BLA library uses column-major matrix notation in code
+  BLA::Matrix<3> eulerVector;
+  eulerVector.Fill(0);
+  eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
+  if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
+    eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
+    eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
+  } else {
+    eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
+    eulerVector(2) = 0;
+  }
+  return eulerVector;
+}
+
+float clamp(float value,float min,float max) {
+  return fmaxf( min, fminf(max, value));
+}