Diff: Gyroscope.cpp

Revision:

0:d884c7453c85

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gyroscope.cpp	Tue May 21 13:50:20 2013 +0000
@@ -0,0 +1,107 @@
+#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;
+}