Diff: Gyroscope.cpp

Revision:

7:604a8369b801

Parent:

6:c12cea26842d

Child:

8:cba37530d480

--- a/Gyroscope.cpp	Thu Feb 12 19:00:28 2015 +0000
+++ b/Gyroscope.cpp	Tue Feb 17 16:53:50 2015 +0000
@@ -2,54 +2,74 @@
 #define DEBUG "BMX055-Gyr"
 #include "Logger.h"
 
-Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) {
+Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3)
+{
     if (powerOn()) {
         INFO("Bosch Sensortec BMX055-Gyro found");
+        powerOff();
     } else {
         WARN("Bosch Sensortec BMX055-Gyro not found");
     }
 }
 
-void Gyroscope::powerOff() {
+void Gyroscope::powerOff()
+{
     write_reg(0x11, 1);
     LOG("deep sleep");
 }
 
-void Gyroscope::handleInterrupt(void) {
+void Gyroscope::handleInterrupt(void)
+{
     static uint32_t ticks = 0;
     Sensor::Data frame = read();
-    
+
     if (ticks % 100 == 0)
         sendData(frame);
 }
 
-bool Gyroscope::powerOn() {
+bool Gyroscope::powerOn()
+{
     write_reg(0x14, 0xB6); // softreset
     wait_ms(30);
     LOG("powered on");
     return read_reg(0x00) == 0x0f; // verify Chip ID
 }
 
-void Gyroscope::start() {
+void Gyroscope::start()
+{
     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() {
+void Gyroscope::stop()
+{
     timer.stop();
     write_reg(0x15, 0); // turn off new data interrupt
 }
 
-Sensor::Data Gyroscope::read() {
+Sensor::Data Gyroscope::read()
+{
     Sensor::Data frame;
     
-    read_reg(0x3F, (uint8_t*)&frame, sizeof frame);
-    frame.timestamp = timer.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.
+    
+    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);
+    
+    write_reg(0x15, 1 << 7); // new data interrupt enabled
     
     return frame;
 }