From 2c6cb137264e7fb35a48bfc6c2bf042f024366a7 Mon Sep 17 00:00:00 2001
From: darevish <dar3vish@gmail.com>
Date: Tue, 16 Dec 2014 00:45:29 +0100
Subject: [PATCH] minimal class and example for L3GD20 gyro

---
 Adafruit_L3GD20/Adafruit_I2C.py    |  1 +
 Adafruit_L3GD20/Adafruit_L3GD20.py | 36 ++++++++++++++++++
 Adafruit_L3GD20/example.py         | 61 ++++++++++++++++++++++++++++++
 3 files changed, 98 insertions(+)
 create mode 120000 Adafruit_L3GD20/Adafruit_I2C.py
 create mode 100755 Adafruit_L3GD20/Adafruit_L3GD20.py
 create mode 100755 Adafruit_L3GD20/example.py

diff --git a/Adafruit_L3GD20/Adafruit_I2C.py b/Adafruit_L3GD20/Adafruit_I2C.py
new file mode 120000
index 00000000..77f06164
--- /dev/null
+++ b/Adafruit_L3GD20/Adafruit_I2C.py
@@ -0,0 +1 @@
+../Adafruit_I2C/Adafruit_I2C.py
\ No newline at end of file
diff --git a/Adafruit_L3GD20/Adafruit_L3GD20.py b/Adafruit_L3GD20/Adafruit_L3GD20.py
new file mode 100755
index 00000000..4d756265
--- /dev/null
+++ b/Adafruit_L3GD20/Adafruit_L3GD20.py
@@ -0,0 +1,36 @@
+#!/usr/bin/python
+
+from Adafruit_I2C import Adafruit_I2C
+
+class L3GD20(Adafruit_I2C):
+
+    L3GD20_ADDRESS = 0x6b
+                                             # Default    Type
+    L3GD20_REGISTER_CTRL_REG1 = 0x20         # 00000111   rw
+    L3GD20_REGISTER_CTRL_REG4 = 0x23         # 00000000   rw
+
+    L3GD20_REGISTER_OUT_X_L = 0x28
+
+    GAIN = 0.07
+
+    def __init__(self, busnum=-1, debug=False):
+
+        self.gyro = Adafruit_I2C(self.L3GD20_ADDRESS, busnum, debug)
+
+        self.gyro.write8(self.L3GD20_REGISTER_CTRL_REG1, 0x0F)
+        self.gyro.write8(self.L3GD20_REGISTER_CTRL_REG4, 0x30)
+  
+
+    def gyro16(self, list, idx):
+        n = (list[idx+1] << 8) | list[idx]
+        return n if n < 32768 else n - 65536
+
+
+    def read(self):
+        list = self.gyro.readList(
+          self.L3GD20_REGISTER_OUT_X_L | 0x80, 6)
+        res = ( self.gyro16(list, 0),
+                 self.gyro16(list, 2),
+                 self.gyro16(list, 4) )
+
+        return res
diff --git a/Adafruit_L3GD20/example.py b/Adafruit_L3GD20/example.py
new file mode 100755
index 00000000..fd80d5c7
--- /dev/null
+++ b/Adafruit_L3GD20/example.py
@@ -0,0 +1,61 @@
+#!/usr/bin/python
+
+from Adafruit_L3GD20 import L3GD20
+
+import time
+
+def getAvgDrift(gyro):
+
+    print 'getAvgDrift begin'
+
+    gyroXAngle = gyroYAngle = gyroZAngle = 0
+
+    sampleCnt = 500
+
+    for i in range(0, sampleCnt):
+        readout = gyro.read()
+        gyroXAngle += readout[0]
+        gyroYAngle += readout[1]
+        gyroZAngle += readout[2]
+        time.sleep(0.02)
+
+    print 'getAvgDrift end'
+        
+    return (
+        gyroXAngle / sampleCnt,
+        gyroYAngle / sampleCnt,
+        gyroZAngle / sampleCnt
+    )
+
+
+if __name__ == '__main__':
+
+    gyro = L3GD20()
+
+    avgDrift = getAvgDrift(gyro)
+
+    gyroXAngle = gyroYAngle = gyroZAngle = 0
+    loopCnt = 0;
+
+    lastTime = time.time() * 1000
+
+    while True:
+
+        readout = gyro.read()
+
+        currentTime = time.time() * 1000
+        timeDiff = currentTime - lastTime
+        lastTime = currentTime
+
+        gyroXAngle += (readout[0] - avgDrift[0]) * gyro.GAIN * (timeDiff / 1000)
+        gyroYAngle += (readout[1] - avgDrift[1]) * gyro.GAIN * (timeDiff / 1000)
+        gyroZAngle += (readout[2] - avgDrift[2]) * gyro.GAIN * (timeDiff / 1000)
+
+
+        if (loopCnt > 0 and (loopCnt % 25) == 0):
+            print 'X: ' + str(gyroXAngle) + ' Y: ' + str(gyroYAngle) + ' Z: ' + str(gyroZAngle)
+            loopCnt = 0
+
+        loopCnt += 1
+
+        time.sleep(0.02)