Diff: Gyroscope.cpp

Revision:

44:5483079fa156

Parent:

40:8e852115fe55

--- a/Gyroscope.cpp	Wed May 27 11:45:00 2015 +0000
+++ b/Gyroscope.cpp	Fri May 29 12:21:31 2015 +0000
@@ -4,7 +4,15 @@
 
 #include "Vector3.h"
 
-Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */),  interruptSet(false), int1(p3), int2(p1), tick(0)
+Gyroscope::Gyroscope(I2C &i2c) :
+    I2CPeripheral(i2c, 0x69 << 1 /* address */),  
+    isCalibrated(false), 
+    interruptSet(false), 
+    numSamples(0),
+    offset(0.0, 0.0, 0.0),
+    int1(p3), 
+    int2(p1), 
+    tick(0)
 {
     if (powerOn()) {
         INFO("Bosch Sensortec BMX055-Gyro found");
@@ -49,13 +57,12 @@
     write_reg(0x15, 0); // turn off new data interrupt
 }
 
-Vector3 Gyroscope::read()
+Vector3 Gyroscope::uncalibrated_read()
 {
     // This chip causes high spikes in the data if FIFO or auto-incremented buffer is read.
     // To work around this, we will read each register ourselves. Also keeping interrupt disabled
     // until we finish reading all bytes.
     //const float gyro_resolution = 0.0610351563; // deg/sec for 2000 deg/sec range
-    const float gyro_resolution = 0.0152587891; // deg/sec for 500 deg/sec range
 
     //write_reg(0x15, 0); // new data interrupt disabled
 
@@ -71,9 +78,42 @@
     const int16_t y = buffer[3] << 8 | buffer[2];
     const int16_t z = buffer[5] << 8 | buffer[4];
 
-    //write_reg(0x15, 1 << 7); // new data interrupt enabled
-    
+    //write_reg(0x15, 1 << 7); // new data interrupt enabled    
     //static Vector3 const offsets(-0.16, -0.17, 0.015);
 
-    return (Vector3(x, y, z) * gyro_resolution);// - offsets;
+    return Vector3(x, y, z);
+}
+
+Vector3 Gyroscope::read()
+{
+    const float gyro_resolution = 0.0152587891;
+    return (uncalibrated_read() - offset) * gyro_resolution;
 }
+
+//http://math.stackexchange.com/questions/106700/incremental-averageing
+void Gyroscope::calibtrationInterrupt(void){
+    const static size_t maxNumCalibrationSamples = 200;
+    
+    Vector3 rawGyroReading = uncalibrated_read();
+        
+    //newMean = oldMean + (currentReading - oldMean) / newNumSamples;
+    offset = offset + (rawGyroReading - offset) / (++numSamples);
+    
+    //Turn off calibration once we read desired number of samples
+    if(numSamples == maxNumCalibrationSamples){
+        numSamples = 0;
+        isCalibrated = true;
+        int1.rise(this, &Gyroscope::handleInterrupt);
+    }
+}
+
+void Gyroscope::calibrate(){
+    int1.rise(this, &Gyroscope::calibtrationInterrupt);
+}
+
+void Gyroscope::reset(){
+    offset.x = 0.0;
+    offset.y = 0.0;
+    offset.z = 0.0;
+
+}
\ No newline at end of file