Fixes to make Gyro more stable

- Changed to using moving window average (8 samples every 5ms) without blocking.

Author: ClutchplateDude

src/Gyro.cpp

@@ -16,6 +16,11 @@ POP_NO_WARNINGS
  * */
 
 bool Gyro::isPresent(false);
+float Gyro::_pitchSamples[WINDOW_SIZE] = {};
+float Gyro::_rollSamples[WINDOW_SIZE]  = {};
+int Gyro::_ringIndex(0);
+int Gyro::_samplesCollected(0);
+unsigned long Gyro::_lastSampleTime(0);
 
 void Gyro::startup()
 /* Starts up the MPU-6050 device.
@@ -54,6 +59,16 @@ void Gyro::startup()
     Wire.write(6);  // 5Hz bandwidth (lowest) for smoothing
     Wire.endTransmission(true);
 
+    // Reset moving average state
+    for (int i = 0; i < WINDOW_SIZE; i++)
+    {
+        _pitchSamples[i] = 0;
+        _rollSamples[i]  = 0;
+    }
+    _ringIndex        = 0;
+    _samplesCollected = 0;
+    _lastSampleTime   = 0;
+
     LOG(DEBUG_INFO, "[GYRO]:: Started");
 }
 
@@ -66,45 +81,69 @@ void Gyro::shutdown()
     // Nothing to do
 }
 
+void Gyro::collectSample()
+/* Reads one accelerometer sample and stores it in the circular buffer.
+*/
+{
+    // Execute 6 byte read from MPU6050_REG_ACCEL_XOUT_H
+    Wire.beginTransmission(MPU6050_I2C_ADDR);
+    Wire.write(MPU6050_REG_ACCEL_XOUT_H);
+    Wire.endTransmission(false);
+    Wire.requestFrom(MPU6050_I2C_ADDR, 6, 1);      // Read 6 registers total, each axis value is stored in 2 registers
+    int16_t AcX = Wire.read() << 8 | Wire.read();  // X-axis value
+    int16_t AcY = Wire.read() << 8 | Wire.read();  // Y-axis value
+    int16_t AcZ = Wire.read() << 8 | Wire.read();  // Z-axis value
+
+    // Calculating the Pitch angle (rotation around Y-axis)
+    _pitchSamples[_ringIndex] = atan2f(-1.0f * AcX, sqrtf((float) AcY * AcY + (float) AcZ * AcZ)) * 180.0f / static_cast<float>(PI);
+    // Calculating the Roll angle (rotation around X-axis)
+    _rollSamples[_ringIndex] = atan2f(-1.0f * AcY, sqrtf((float) AcX * AcX + (float) AcZ * AcZ)) * 180.0f / static_cast<float>(PI);
+
+    _ringIndex = (_ringIndex + 1) % WINDOW_SIZE;
+    if (_samplesCollected < WINDOW_SIZE)
+    {
+        _samplesCollected++;
+    }
+}
+
 angle_t Gyro::getCurrentAngles()
 /* Returns roll & tilt angles from MPU-6050 device in angle_t object in degrees.
-   If MPU-6050 is not found then returns {0,0}.
+   Non-blocking: collects one sample per call if at least SAMPLE_INTERVAL ms
+   have elapsed since the last sample. Returns the moving average over the
+   last WINDOW_SIZE samples.
 */
 {
-    const int windowSize = 16;
-    // Read the accelerometer data
-    struct angle_t result;
-    result.pitchAngle = 0;
-    result.rollAngle  = 0;
+    angle_t result;
+
     if (!isPresent)
-        return result;  // Gyro is not available
+        return result;
+
+    unsigned long now = millis();
+    if (now - _lastSampleTime >= SAMPLE_INTERVAL)
+    {
+        _lastSampleTime = now;
+        collectSample();
+    }
 
-    for (int i = 0; i < windowSize; i++)
+    if (_samplesCollected == 0)
+        return result;
+
+    float pitchSum = 0;
+    float rollSum  = 0;
+    for (int i = 0; i < _samplesCollected; i++)
     {
-        // Execute 6 byte read from MPU6050_REG_WHO_AM_I
-        Wire.beginTransmission(MPU6050_I2C_ADDR);
-        Wire.write(MPU6050_REG_ACCEL_XOUT_H);
-        Wire.endTransmission(false);
-        Wire.requestFrom(MPU6050_I2C_ADDR, 6, 1);      // Read 6 registers total, each axis value is stored in 2 registers
-        int16_t AcX = Wire.read() << 8 | Wire.read();  // X-axis value
-        int16_t AcY = Wire.read() << 8 | Wire.read();  // Y-axis value
-        int16_t AcZ = Wire.read() << 8 | Wire.read();  // Z-axis value
-
-        // Calculating the Pitch angle (rotation around Y-axis)
-        result.pitchAngle += atan2f(-1.0f * AcX, sqrtf((float) AcY * AcY + (float) AcZ * AcZ)) * 180.0f / static_cast<float>(PI);
-        // Calculating the Roll angle (rotation around X-axis)
-        result.rollAngle += atan2f(-1.0f * AcY, sqrtf((float) AcX * AcX + (float) AcZ * AcZ)) * 180.0f / static_cast<float>(PI);
-
-        delay(10);  // Decorrelate measurements
+        pitchSum += _pitchSamples[i];
+        rollSum += _rollSamples[i];
     }
+    result.pitchAngle = pitchSum / _samplesCollected;
+    result.rollAngle  = rollSum / _samplesCollected;
 
-    result.pitchAngle /= windowSize;
-    result.rollAngle /= windowSize;
     #if GYRO_AXIS_SWAP == 1
     float temp        = result.pitchAngle;
     result.pitchAngle = result.rollAngle;
     result.rollAngle  = temp;
     #endif
+
     return result;
 }
 

src/Gyro.hpp

@@ -15,6 +15,8 @@ class Gyro
     static float getCurrentTemperature();
 
   private:
+    static void collectSample();
+
     // MPU6050 constants
     enum
     {
@@ -28,6 +30,15 @@ class Gyro
         MPU6050_REG_WHO_AM_I     = 0x75
     };
 
-    static bool isPresent;  // True if gyro correctly detected on startup
+    // Use about 72 bytes of RAM for the moving average buffer
+    static const int WINDOW_SIZE              = 8;  // Number of samples to keep for moving average
+    static const unsigned long SAMPLE_INTERVAL = 5;  // ms between samples
+
+    static bool isPresent;
+    static float _pitchSamples[WINDOW_SIZE];
+    static float _rollSamples[WINDOW_SIZE];
+    static int _ringIndex;
+    static int _samplesCollected;  // Counts up to WINDOW_SIZE, then stays there
+    static unsigned long _lastSampleTime;
 };
 #endif