Update readme, WIP euler angles, calibration quaternion

README.org

@@ -42,6 +42,7 @@
 ** dependencies (libraries)
 1. OSC (filter by "open sound control")
 2. MPU6050
+3. BasicLinearAlgebra
 
 ** calibration
    - IMU_Zero

osc32final/osc32final.ino

@@ -14,7 +14,7 @@ using namespace BLA;
 
 #define SERIAL_OSC
 //#define WIFI_OSC
-//#define BT_OSC
+#define BT_OSC
 
 #define OUTPUT_READABLE_WORLDACCEL
 
@@ -107,6 +107,7 @@ uint8_t fifoBuffer[64]; // FIFO storage buffer
 Quaternion q;           // [w, x, y, z]         quaternion container
 Quaternion pq;          // [w, x, y, z]         previous quaternion container
 Quaternion diff;        // [w, x, y, z]         quaternion derivate container
+Quaternion cq;          // [w, x, y, z]         calibration quaternion
 VectorInt16 aa;         // [x, y, z]            accel sensor measurements
 VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
@@ -117,6 +118,9 @@ float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gra
 uint32_t timeOn = 0;    //                      Uptime counter for movement calculation
 Matrix<3> position;     // [x,y,z]              tracks position of device
 Matrix<3> speed;        // [x,y,z]              tracks speed of device
+Matrix<3> eulerVector;
+Matrix<3> eulerDiffVector;
+
 
 // Sem dobimo vrednosti pospeskomerja in ziroskopa
 int16_t AcX,AcY,AcZ;
@@ -127,6 +131,37 @@ byte keys[] = {16, 17, 5, 18};
 byte pressed[] = {0, 0, 0, 0};
 byte KEYLEN = 4;
 
+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));
+}
+
 
 /* OSC MSG channels */
 OSCBundle bundle;
@@ -241,16 +276,25 @@ void loop() {
     
     // get quaternion values in easy matrix form: w x y z
     mpu.dmpGetQuaternion(&q, fifoBuffer);
-  
+    q = q.getProduct(cq);
     
     //compute the differential rotation between the previous and new orientation
     diff = q.getProduct(pq.getConjugate());
     
     // Quaternion - rotacija
-    bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); //  W X Y Z
+    bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W X Y Z
+
+    // Euler - rotacija
+    eulerVector = eulerFromQuaternion(q);
+    bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
+    
     // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
     bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z
-  
+
+    // Rotation diff value in euler angle
+    eulerDiffVector = eulerFromQuaternion(diff);
+    bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z
+    
 #ifdef OUTPUT_READABLE_REALACCEL
     // display real acceleration, adjusted to remove gravity
     mpu.dmpGetAccel(&aa, fifoBuffer);
@@ -297,6 +341,11 @@ void loop() {
       pressed[i] = !digitalRead(keys[i]);
       bundle.getOSCMessage("/keys")->add(pressed[i]);
     }
+
+    // Reset calibration euler?
+    if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) {
+      cq = q.getConjugate();
+    }
     
 #ifdef SERIAL_OSC
     SLIPSerial.beginPacket();