Diff: test.cpp

Revision:

41:3ead1dd2cc3a

Parent:

40:6fa672be85ec

Child:

42:96671b71aac5

--- a/test.cpp	Thu Jul 25 02:53:34 2019 +0000
+++ b/test.cpp	Thu Jul 25 03:20:41 2019 +0000
@@ -324,6 +324,50 @@
     }
 }
 
+void testL3gAndCalibrate()
+{
+    wait_ms(2000);
+    Pacer reportPacer(750000);
+    Pacer readingPacer(2000);
+    Timer timer;
+    timer.start();
+    int32_t total = 0;
+    int32_t readingCount = 0;
+    while(1)
+    {
+        if (readingPacer.pace())
+        {
+          int32_t result = l3gZAvailable();
+          if (result == 1)
+          {
+              int32_t gz = l3gZRead();
+              if (gz < -500000)
+              {
+                  pc.printf("l3gZRead error: %d\n", gz);
+              }
+              else
+              {
+                  total += gz;
+                  readingCount += 1;
+              }
+          }
+          else if (result != 0)
+          {
+              pc.printf("l3gZAvailable error: %d\n", result);
+          }
+        }
+
+        if (reportPacer.pace())
+        {
+            float average = (float)total / readingCount;
+            pc.printf("%d, %d, %f\r\n", total, readingCount, average);
+        }
+    }
+    
+    // Gyro calibration results get hardcoded into TurnSensor::update()
+    // for now until we figure out something better.
+}
+
 void testL3g()
 {
     Pacer reportPacer(750000);