aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py30
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