ITG3200HL.cpp

00001 #include "ITG3200HL.h"
00002 
00003 ITG3200HL::~ITG3200HL()
00004 {
00005     delete[] output;
00006     delete gyro;
00007 }
00008 
00009 void ITG3200HL::init(int calibsampls, int readsampls, float samplrate)
00010 {
00011     gyro=new ITG3200(p28,p27);
00012     gyro->setLpBandwidth(LPFBW_42HZ);
00013     output=new double[3];
00014     calibrationsamples=calibsampls;
00015     samplerate=samplrate;
00016     readsamples=readsampls;
00017     output[0]=0;
00018     output[1]=0;
00019     output[2]=0;
00020 }
00021 
00022 void ITG3200HL::calibrate()
00023 {
00024     double a_xAccumulator = 0;
00025     double a_yAccumulator = 0;
00026     double a_zAccumulator = 0;
00027 
00028     //Take a number of readings and average them
00029     //to calculate the zero g offset.
00030     for (int i = 0; i < calibrationsamples; i++) {
00031         int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()};
00032 
00033         a_xAccumulator += (int16_t) readings[0];
00034         a_yAccumulator += (int16_t) readings[1];
00035         a_zAccumulator += (int16_t) readings[2];
00036 
00037         wait(samplerate);
00038 
00039     }
00040 
00041     a_xAccumulator /= calibrationsamples;
00042     a_yAccumulator /= calibrationsamples;
00043     a_zAccumulator /= calibrationsamples;
00044 
00045     //At 4mg/LSB, 250 LSBs is 1g.
00046     xBias = a_xAccumulator;
00047     yBias = a_yAccumulator;
00048     zBias=a_zAccumulator;
00049     //zBias = (a_zAccumulator - 250);
00050 
00051     a_xAccumulator = 0;
00052     a_yAccumulator = 0;
00053     a_zAccumulator = 0;
00054 }
00055 
00056 double* ITG3200HL::sample()
00057 {
00058     double a_xAccumulator = 0;
00059     double a_yAccumulator = 0;
00060     double a_zAccumulator = 0;
00061     for (int i=0; i<readsamples; ++i) {
00062         int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()};
00063         a_xAccumulator += (int16_t) readings[0];
00064         a_yAccumulator += (int16_t) readings[1];
00065         a_zAccumulator += (int16_t) readings[2];
00066     }
00067     //Average the samples, remove the bias, and calculate the rotation
00068     //in rad/s
00069     output[0] = ((a_xAccumulator / readsamples) - xBias) * GYRO_GAIN;
00070     output[1] = ((a_yAccumulator / readsamples) - yBias) * GYRO_GAIN;
00071     output[2] = ((a_zAccumulator / readsamples) - zBias) * GYRO_GAIN;
00072     return output;
00073 }