From c7c5e4fa96237c42b186009c0498379be8678e6c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= <jurij@kompot.si>
Date: Sat, 5 Mar 2022 19:07:01 +0100
Subject: [PATCH] WIP calibration, euler angles

---
 osc32final/osc32final.ino | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino
index 0dd365f..99944de 100644
--- a/osc32final/osc32final.ino
+++ b/osc32final/osc32final.ino
@@ -120,6 +120,7 @@ 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;
+bool reset; // For quaternion calibration
 
 
 // Sem dobimo vrednosti pospeskomerja in ziroskopa
@@ -285,15 +286,15 @@ void loop() {
     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
+    //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
+    //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
@@ -318,6 +319,7 @@ void loop() {
 #endif
 
    // Calculate speed and position from accelerometer data
+   /*
     int prevTime = timeOn;
     timeOn = millis();
     int elapsedTime = timeOn - prevTime;
@@ -329,7 +331,7 @@ void loop() {
 
     bundle.add("/position/").add(position(0)).add(position(1)).add(position(2));
     bundle.add("/speed/").add(speed(0)).add(speed(1)).add(speed(2));
-
+*/
     // Accelerometer
     bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z
 
@@ -344,7 +346,15 @@ void loop() {
 
     // Reset calibration euler?
     if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) {
-      cq = q.getConjugate();
+      if (!reset) {
+        cq = q.getConjugate();
+        reset = true;
+        Serial.println("Quaternion calibrate");
+      }
+    } else {
+      if (reset) {
+        reset = false;
+      }
     }
     
 #ifdef SERIAL_OSC