Diff: l3g.cpp

Revision:

42:96671b71aac5

Parent:

40:6fa672be85ec

--- a/l3g.cpp	Thu Jul 25 03:20:41 2019 +0000
+++ b/l3g.cpp	Sat Jul 27 20:58:46 2019 +0000
@@ -6,7 +6,7 @@
 
 I2C i2c(p9, p10);
 
-int address = 0xD6;
+const int address = 0xD6;
 
 int32_t l3gInit()
 {
@@ -92,4 +92,49 @@
     }
     
     return (int16_t)(data[1] << 8 | data[0]);
+}
+
+// The stuff below doesn't actually have anything to do with the L3G.
+
+#define L3G_CAL_COUNT_MAX 1000
+int8_t l3gCalBuffer[L3G_CAL_COUNT_MAX];
+uint32_t l3gCalCount = 0;
+uint32_t l3gCalReplayIndex = 0;
+
+int32_t l3gCalibrate()
+{
+    int32_t reading = l3gZRead();
+    
+    if (l3gCalCount < L3G_CAL_COUNT_MAX)
+    {
+        int8_t c;
+        if (reading > 127) { c = 127; }
+        else if (reading < -128) { c = -128; }
+        else { c = reading; }
+        l3gCalBuffer[l3gCalCount++] = c;
+    }
+    
+    return reading;
+}
+
+bool l3gCalibrateDone()
+{
+    return l3gCalCount >= L3G_CAL_COUNT_MAX;
+}
+
+void l3gCalibrateReset()
+{
+    l3gCalCount = 0;
+}
+
+int32_t l3gZReadCalibrated()
+{
+    int8_t zeroRate = 0;
+    if (l3gCalCount)
+    {
+      if (l3gCalReplayIndex >= l3gCalCount) { l3gCalReplayIndex = 0; }
+      zeroRate = l3gCalBuffer[l3gCalReplayIndex++];
+    }
+    
+    return l3gZRead() - zeroRate;
 }
\ No newline at end of file