Diff: Gyroscope.cpp

Revision:

40:8e852115fe55

Parent:

39:1fa9c0e1ffde

Child:

44:5483079fa156

Child:

46:fd5a62296b12

--- a/Gyroscope.cpp	Tue May 19 14:18:30 2015 +0000
+++ b/Gyroscope.cpp	Tue May 26 11:28:37 2015 +0000
@@ -4,7 +4,7 @@
 
 #include "Vector3.h"
 
-Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3), int2(p1), tick(0)
+Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */),  interruptSet(false), int1(p3), int2(p1), tick(0)
 {
     if (powerOn()) {
         INFO("Bosch Sensortec BMX055-Gyro found");
@@ -36,6 +36,7 @@
 void Gyroscope::start()
 {
     write_reg(0x10, 5); // set capture rate: 100 Hz / filter: 12 Hz
+    write_reg(0x0F, 2); // set range +/- 500 deg/sec
     write_reg(0x16, 5); // interrupts active high, push-pull
     write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro)
     int1.rise(this, &Gyroscope::handleInterrupt);
@@ -53,19 +54,26 @@
     // 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
+    //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
 
     uint8_t buffer[6];
+    /*
     for (size_t i = 0; i < 6; i++)
         buffer[i] = read_reg(0x02 + i);
-
+    */
+    
+    read_reg(0x02, buffer, sizeof buffer);
+    
     const int16_t x = buffer[1] << 8 | buffer[0];
     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
+    
+    //static Vector3 const offsets(-0.16, -0.17, 0.015);
 
-    return Vector3(x, y, z) * gyro_resolution;
+    return (Vector3(x, y, z) * gyro_resolution);// - offsets;
 }