From e9231a2c77b4003ac948a9925828931692163fca Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Tue, 2 Apr 2019 20:55:29 +0200
Subject: [PATCH] [X-LiteS] Gyro fixes (ouille ouille !)

---
 radio/src/gyro.cpp | 14 +++++++-------
 radio/src/gyro.h   |  2 +-
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/radio/src/gyro.cpp b/radio/src/gyro.cpp
index 121735561..5fa2594ff 100644
--- a/radio/src/gyro.cpp
+++ b/radio/src/gyro.cpp
@@ -24,19 +24,19 @@
 
 Gyro gyro;
 
-void GyroBuffer::read(int32_t values[GYRO_SAMPLES_COUNT])
+void GyroBuffer::read(int32_t values[GYRO_VALUES_COUNT])
 {
-  for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
-    sums[index] -= samples[index].values[i];
-  }
-
   index = (index + 1) & (GYRO_SAMPLES_COUNT - 1);
 
+  for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
+    sums[i] -= samples[index].values[i];
+  }
+
   gyroRead(samples[index].raw);
 
   for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
-    sums[index] += samples[index].values[i];
-    values[i] = sums[index] >> GYRO_SAMPLES_EXPONENT;
+    sums[i] += samples[index].values[i];
+    values[i] = sums[i] >> GYRO_SAMPLES_EXPONENT;
   }
 }
 
diff --git a/radio/src/gyro.h b/radio/src/gyro.h
index db29b0836..b4b40ac78 100644
--- a/radio/src/gyro.h
+++ b/radio/src/gyro.h
@@ -35,7 +35,7 @@ class GyroBuffer {
     uint8_t index;
 
   public:
-    void read(int32_t values[GYRO_SAMPLES_COUNT]);
+    void read(int32_t values[GYRO_VALUES_COUNT]);
 };
 
 class Gyro {