From 76f1391303a921a25435a051ec1c4077f8097822 Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Wed, 2 Nov 2022 20:23:22 +0100
Subject: Adding gyroscope integration feature to get rotation.

---
 .../TobiiGlassesPro2/TobiiInertialMeasureUnit.py   | 30 ++++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
index 6bc89f7..1328389 100644
--- a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
+++ b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
@@ -70,6 +70,36 @@ class TobiiInertialMeasureUnit():
 
         return TobiiData.Gyroscope(gyroscope_data_object.value - self.__gyroscope_offset)
 
+    def reset_rotation(self, ts = 0, value = [0, 0, 0]):
+
+        self.__last_gyroscope_ts = ts
+        self.__last_gyroscope = value
+        self.__rotation = value
+
+    def update_rotation(self, gyroscope_data_ts, gyroscope_data_object):
+
+        # Convert deg/s into deg/ms
+        current_gyroscope = gyroscope_data_object.value * 1e-3
+
+        # Init gyroscope integration
+        if self.__last_gyroscope_ts == 0:
+
+            self.reset_rotation(gyroscope_data_ts, current_gyroscope)
+
+        # Calculate elapsed time in ms
+        delta_time = (gyroscope_data_ts - self.__last_gyroscope_ts) / 1e3
+
+        # Integrate gyroscope
+        self.__rotation += self.__last_gyroscope * delta_time
+
+        # Store current as last
+        self.__last_gyroscope_ts = gyroscope_data_ts
+        self.__last_gyroscope = current_gyroscope
+
+    def get_rotation(self):
+
+        return self.__rotation
+
     def _accelerometer_linear_fit(self, x, a, b):
 
         return a * x + b
-- 
cgit v1.1