Gyroscope.cpp

00001 #include "Gyroscope.h"
00002 
00003 Gyroscope::Gyroscope(ITG3205* gyroHardware, float sensitivity, float dataRate)
00004 {
00005     this->gyroHardware = gyroHardware;
00006     this->sensitivity = sensitivity;
00007     this->dataRate = dataRate;
00008     
00009     updateZeroRates();
00010 }
00011 
00012 Gyroscope::~Gyroscope(void)
00013 {
00014 }
00015 
00016 void Gyroscope::updateZeroRates()
00017 {
00018     update(10, 10);
00019     
00020     zeroRateX = rawX;
00021     zeroRateY = rawY;
00022     zeroRateZ = rawZ;
00023     
00024     resetAngles();
00025 }
00026 
00027 void Gyroscope::update()
00028 {    
00029     update(1, 1);
00030 }
00031 
00032 void Gyroscope::update(int samplesSize, int sampleDataRate)
00033 {
00034     //int axes[3] = {0, 0, 0};
00035     
00036     rawX = 0;
00037     rawY = 0;
00038     rawZ = 0;
00039     
00040     for (int i = 0; i < samplesSize; i++)
00041     {
00042         //(gyroHardware)->getAxes(axes);
00043         while(!gyroHardware->isRawReady());
00044         
00045         rawX += ( gyroHardware->getGyroX() / samplesSize);
00046         rawY += ( gyroHardware->getGyroY() / samplesSize);
00047         rawZ += ( gyroHardware->getGyroZ() / samplesSize);
00048         
00049         wait_us(sampleDataRate);
00050     }
00051     
00052     angleX += getDegreesX() * dataRate;
00053     angleY += getDegreesY() * dataRate;
00054     angleZ += getDegreesZ() * dataRate;
00055 }
00056 
00057 void Gyroscope::resetAngles()
00058 {
00059     angleX = 0;
00060     angleY = 0;
00061     angleZ = 0;
00062 }
00063 
00064 float Gyroscope::getDegreesX()
00065 {
00066     return (rawX - zeroRateX) / sensitivity;
00067 }
00068 
00069 float Gyroscope::getDegreesY()
00070 {
00071     return (rawY - zeroRateY) / sensitivity;
00072 }
00073 
00074 float Gyroscope::getDegreesZ()
00075 {
00076     return (rawZ - zeroRateZ) / sensitivity;
00077 }
00078 
00079 float Gyroscope::getRadiansX()
00080 {
00081     return getDegreesX() / 180.0 * PI;
00082 }
00083 
00084 float Gyroscope::getRadiansY()
00085 {
00086     return getDegreesY() / 180.0 * PI;
00087 }
00088 
00089 float Gyroscope::getRadiansZ()
00090 {
00091     return getDegreesZ() / 180.0 * PI;
00092 }
00093 
00094 float Gyroscope::getAngleX()
00095 {
00096     return angleX;
00097 }
00098 
00099 float Gyroscope::getAngleY()
00100 {
00101     return angleY;
00102 }
00103 
00104 float Gyroscope::getAngleZ()
00105 {
00106     return angleZ;
00107 }