diff options
author | Théo de la Hogue | 2022-11-02 20:23:22 +0100 |
---|---|---|
committer | Théo de la Hogue | 2022-11-02 20:23:22 +0100 |
commit | 76f1391303a921a25435a051ec1c4077f8097822 (patch) | |
tree | 39a92ecfdabeef16430e5740b88851a8032e0c9a | |
parent | 9a8b2c598bddcfa4bb41032f03dd264394913201 (diff) | |
download | argaze-76f1391303a921a25435a051ec1c4077f8097822.zip argaze-76f1391303a921a25435a051ec1c4077f8097822.tar.gz argaze-76f1391303a921a25435a051ec1c4077f8097822.tar.bz2 argaze-76f1391303a921a25435a051ec1c4077f8097822.tar.xz |
Adding gyroscope integration feature to get rotation.
-rw-r--r-- | src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py | 30 |
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 |