Diff: ITG3200HL.cpp

Revision:

0:441caaf895d8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200HL.cpp	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,73 @@
+#include "ITG3200HL.h"
+
+ITG3200HL::~ITG3200HL()
+{
+    delete[] output;
+    delete gyro;
+}
+
+void ITG3200HL::init(int calibsampls, int readsampls, float samplrate)
+{
+    gyro=new ITG3200(p28,p27);
+    gyro->setLpBandwidth(LPFBW_42HZ);
+    output=new double[3];
+    calibrationsamples=calibsampls;
+    samplerate=samplrate;
+    readsamples=readsampls;
+    output[0]=0;
+    output[1]=0;
+    output[2]=0;
+}
+
+void ITG3200HL::calibrate()
+{
+    double a_xAccumulator = 0;
+    double a_yAccumulator = 0;
+    double a_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the zero g offset.
+    for (int i = 0; i < calibrationsamples; i++) {
+        int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()};
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        wait(samplerate);
+
+    }
+
+    a_xAccumulator /= calibrationsamples;
+    a_yAccumulator /= calibrationsamples;
+    a_zAccumulator /= calibrationsamples;
+
+    //At 4mg/LSB, 250 LSBs is 1g.
+    xBias = a_xAccumulator;
+    yBias = a_yAccumulator;
+    zBias=a_zAccumulator;
+    //zBias = (a_zAccumulator - 250);
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+}
+
+double* ITG3200HL::sample()
+{
+    double a_xAccumulator = 0;
+    double a_yAccumulator = 0;
+    double a_zAccumulator = 0;
+    for (int i=0; i<readsamples; ++i) {
+        int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()};
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+    }
+    //Average the samples, remove the bias, and calculate the rotation
+    //in rad/s
+    output[0] = ((a_xAccumulator / readsamples) - xBias) * GYRO_GAIN;
+    output[1] = ((a_yAccumulator / readsamples) - yBias) * GYRO_GAIN;
+    output[2] = ((a_zAccumulator / readsamples) - zBias) * GYRO_GAIN;
+    return output;
+}
\ No newline at end of file