Diff: Gyroscope.cpp

Revision:

8:cba37530d480

Parent:

7:604a8369b801

Child:

11:d21275e60ebb

--- a/Gyroscope.cpp	Tue Feb 17 16:53:50 2015 +0000
+++ b/Gyroscope.cpp	Wed Feb 18 15:13:41 2015 +0000
@@ -2,6 +2,8 @@
 #define DEBUG "BMX055-Gyr"
 #include "Logger.h"
 
+#include "Vector3.h"
+
 Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3)
 {
     if (powerOn()) {
@@ -20,11 +22,7 @@
 
 void Gyroscope::handleInterrupt(void)
 {
-    static uint32_t ticks = 0;
-    Sensor::Data frame = read();
-
-    if (ticks % 100 == 0)
-        sendData(frame);
+    sendData(read());
 }
 
 bool Gyroscope::powerOn()
@@ -40,36 +38,29 @@
     write_reg(0x10, 5); // set capture rate: 200 Hz / filter: 23 Hz
     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)
-    //write_reg(0x18, 1 << 2); // map fifo interrupt to INT3 pin
     int1.rise(this, &Gyroscope::handleInterrupt);
-    timer.stop();
-    timer.reset();
-    timer.start();
     write_reg(0x15, 1 << 7); // new data interrupt enabled
-    //write_reg(0x15, 1 << 6); // fifo interrupt enabled
 }
 
 void Gyroscope::stop()
 {
-    timer.stop();
     write_reg(0x15, 0); // turn off new data interrupt
 }
 
-Sensor::Data Gyroscope::read()
+Vector3 Gyroscope::read()
 {
-    Sensor::Data frame;
-    
     // 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
+
     write_reg(0x15, 0); // new data interrupt disabled
     
-    frame.x = read_reg(0x02) | (read_reg(0x03) << 8);
-    frame.y = read_reg(0x04) | (read_reg(0x05) << 8);
-    frame.z = read_reg(0x06) | (read_reg(0x07) << 8);
-    
+    const int16_t gx = read_reg(0x02) | (read_reg(0x03) << 8);
+    const int16_t gy = read_reg(0x04) | (read_reg(0x05) << 8);
+    const int16_t gz = read_reg(0x06) | (read_reg(0x07) << 8);
+
     write_reg(0x15, 1 << 7); // new data interrupt enabled
-    
-    return frame;
+
+    return Vector3(gx, gy, gz) * gyro_resolution;
 }