Adding gyroscope integration feature to get rotation.

1 files changed, 30 insertions, 0 deletions

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