diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/utils/tobii_stream_arcube_display.py | 14 |
1 files changed, 6 insertions, 8 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py index d307206..9cd89d9 100644 --- a/src/argaze/utils/tobii_stream_arcube_display.py +++ b/src/argaze/utils/tobii_stream_arcube_display.py @@ -111,7 +111,7 @@ def main(): ''' last_gyroscope = numpy.zeros(3) last_gyroscope_ts_ms = 0 - gyroscope_drift = numpy.zeros(3) + gyroscope_offset = numpy.zeros(3) head_rotation = numpy.zeros(3) smooth_factor = 0.5 @@ -134,7 +134,7 @@ def main(): nonlocal last_gyroscope nonlocal last_gyroscope_ts_ms - nonlocal gyroscope_drift + nonlocal gyroscope_offset nonlocal head_rotation # Convert deg/s into deg/ms @@ -150,16 +150,14 @@ def main(): delta_time = data_ts_ms - last_gyroscope_ts_ms gyroscope_derivation = (current_gyroscope - last_gyroscope) / delta_time if delta_time > 0 else numpy.zeros(3) - # Update gyroscope drift smoothly and reset head rotation when gyroscope is stable + # Update gyroscope offset smoothly and reset head rotation when gyroscope is stable if numpy.linalg.norm(gyroscope_derivation) < 1e-5: - gyroscope_drift = gyroscope_drift * smooth_factor + current_gyroscope * (1 - smooth_factor) + gyroscope_offset = gyroscope_offset * smooth_factor + current_gyroscope * (1 - smooth_factor) head_rotation = numpy.zeros(3) - print(f'> gyroscope_drift={gyroscope_drift}') - - # Integrate gyroscope with drift compensation - head_rotation += (last_gyroscope - gyroscope_drift) * delta_time + # Integrate gyroscope with offset compensation + head_rotation += (last_gyroscope - gyroscope_offset) * delta_time # Store current as last last_gyroscope = current_gyroscope |