Update script.js

OCSYT · Feb 12, 2024 · d140574 · d140574
1 parent c9bdec1
commit d140574
Showing 1 changed file with 32 additions and 12 deletions.
diff --git a/tracker/script.js b/tracker/script.js
@@ -264,7 +264,7 @@ async function connectToDevice() {
         trackerdevices[device.id] = [device, null];
         battery[device.id] = 0;
 
-        
+
         trackercount.innerHTML = "Connected Trackers: " + Object.values(trackerdevices).length;
 
         const sensor_characteristic = await sensor_service.getCharacteristic('00dbf1c6-90aa-11ed-a1eb-0242ac120002');
@@ -484,6 +484,7 @@ let initialRotations = {};
 let initialAccel = {};
 let startTimes = {};
 let calibrated = {};
+let driftvalues = {};
 let trackerrotation = {};
 let trackeraccel = {};
 const DriftInterval = 60000;
@@ -543,24 +544,41 @@ function decodeIMUPacket(device, rawdata) {
 
     if (elapsedTime >= DriftInterval) {
         if (!calibrated[deviceId]) {
-            const rotationDifference = calculateRotationDifference(new Quaternion(initialRotations[deviceId].w, initialRotations[deviceId].x, initialRotations[deviceId].y, initialRotations[deviceId].z).toEuler("XYZ"),
-                new Quaternion(rotation.w, rotation.x,
-                    rotation.y, rotation.z).toEuler("XYZ"));
-            const adjustedRotation = rotationDifference;
-            calibrated[deviceId] = adjustedRotation;
+            calibrated[deviceId] = driftvalues[deviceId];
 
             console.log(adjustedRotation);
         }
     }
+
+    if (elapsedTime < DriftInterval) {
+        if (!driftvalues[deviceId]) {
+            driftvalues[deviceId] = { pitch: 0, roll: 0, yaw: 0 };
+        }
+
+        const rotationDifference = calculateRotationDifference(
+            new Quaternion(initialRotations[deviceId].w, initialRotations[deviceId].x, initialRotations[deviceId].y, initialRotations[deviceId].z).toEuler("XYZ"),
+            new Quaternion(rotation.w, rotation.x, rotation.y, rotation.z).toEuler("XYZ")
+        );
+
+        const prevMagnitude = Math.sqrt(driftvalues[deviceId].pitch ** 2 + driftvalues[deviceId].roll ** 2 + driftvalues[deviceId].yaw ** 2);
+        const currMagnitude = Math.sqrt(rotationDifference.pitch ** 2 + rotationDifference.roll ** 2 + rotationDifference.yaw ** 2);
+
+        if (currMagnitude > prevMagnitude) {
+            driftvalues[deviceId] = rotationDifference;
+            console.log(driftvalues[deviceId]);
+        }
+    }
+
+
     if (elapsedTime >= DriftInterval && calibrated[deviceId]) {
         const driftCorrection = {
             pitch: calibrated[deviceId].pitch * (elapsedTime / DriftInterval) % (2 * Math.PI),
             roll: calibrated[deviceId].roll * (elapsedTime / DriftInterval) % (2 * Math.PI),
-            yaw: calibrated[deviceId].yaw * (elapsedTime / DriftInterval) % (2 * Math.PI), 
+            yaw: calibrated[deviceId].yaw * (elapsedTime / DriftInterval) % (2 * Math.PI),
         };
-        
+
         const rotQuat = new Quaternion([rotation.w, rotation.x, rotation.y, rotation.z]);
-    
+
         // Apply drift correction to the rotation quaternion
         const rotationDriftRaw = rotQuat.clone().rotateVector([driftCorrection.pitch, driftCorrection.roll, driftCorrection.yaw]);
         const rotationDrift = Quaternion.fromEuler(rotationDriftRaw[0] % (2 * Math.PI), rotationDriftRaw[1] % (2 * Math.PI), rotationDriftRaw[2] % (2 * Math.PI), "XYZ");
@@ -569,7 +587,7 @@ function decodeIMUPacket(device, rawdata) {
         console.log("Applied fix");
         console.log(rotation);
         console.log(rotationDriftCorrected);
-        
+
         return [device, rotationDriftCorrected, gravity];
     }
     // Return original rotation data
@@ -593,15 +611,16 @@ function CalibrateDrift() {
     for (const deviceId in trackerrotation) {
         if (!initialRotations[deviceId]) {
             delete calibrated[deviceId];
+            delete driftvalues[deviceId];
             initialRotations[deviceId] = trackerrotation[deviceId];
             startTimes[deviceId] = Date.now();
             initialAccel[deviceId] = trackeraccel[deviceId];
         }
     }
-    setTimeout(function() {
+    setTimeout(function () {
         status.innerHTML = "Status: Finished Calibrating";
     }, DriftInterval);
-       
+
 }
 function RemoveDriftOffsets() {
     const status = document.getElementById("calibratingstatus");
@@ -610,6 +629,7 @@ function RemoveDriftOffsets() {
         delete initialRotations[deviceId];
         delete startTimes[deviceId];
         delete initialAccel[deviceId];
+        delete driftvalues[deviceId];
     }
     status.innerHTML = "Status: No Calibration set, using default rotation.";
 }