File content as of revision 0:441caaf895d8:

#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;
}