File content as of revision 0:d884c7453c85:

#include "Gyroscope.h"

Gyroscope::Gyroscope(ITG3205* gyroHardware, float sensitivity, float dataRate)
{
    this->gyroHardware = gyroHardware;
    this->sensitivity = sensitivity;
    this->dataRate = dataRate;
    
    updateZeroRates();
}

Gyroscope::~Gyroscope(void)
{
}

void Gyroscope::updateZeroRates()
{
    update(10, 10);
    
    zeroRateX = rawX;
    zeroRateY = rawY;
    zeroRateZ = rawZ;
    
    resetAngles();
}

void Gyroscope::update()
{    
    update(1, 1);
}

void Gyroscope::update(int samplesSize, int sampleDataRate)
{
    //int axes[3] = {0, 0, 0};
    
    rawX = 0;
    rawY = 0;
    rawZ = 0;
    
    for (int i = 0; i < samplesSize; i++)
    {
        //(gyroHardware)->getAxes(axes);
        while(!gyroHardware->isRawReady());
        
        rawX += ( gyroHardware->getGyroX() / samplesSize);
        rawY += ( gyroHardware->getGyroY() / samplesSize);
        rawZ += ( gyroHardware->getGyroZ() / samplesSize);
        
        wait_us(sampleDataRate);
    }
    
    angleX += getDegreesX() * dataRate;
    angleY += getDegreesY() * dataRate;
    angleZ += getDegreesZ() * dataRate;
}

void Gyroscope::resetAngles()
{
    angleX = 0;
    angleY = 0;
    angleZ = 0;
}

float Gyroscope::getDegreesX()
{
    return (rawX - zeroRateX) / sensitivity;
}

float Gyroscope::getDegreesY()
{
    return (rawY - zeroRateY) / sensitivity;
}

float Gyroscope::getDegreesZ()
{
    return (rawZ - zeroRateZ) / sensitivity;
}

float Gyroscope::getRadiansX()
{
    return getDegreesX() / 180.0 * PI;
}

float Gyroscope::getRadiansY()
{
    return getDegreesY() / 180.0 * PI;
}

float Gyroscope::getRadiansZ()
{
    return getDegreesZ() / 180.0 * PI;
}

float Gyroscope::getAngleX()
{
    return angleX;
}

float Gyroscope::getAngleY()
{
    return angleY;
}

float Gyroscope::getAngleZ()
{
    return angleZ;
}