Diff: Gyroscope.cpp

Revision:

6:c12cea26842d

Parent:

5:b9f2f62a8f90

Child:

7:604a8369b801

--- a/Gyroscope.cpp	Thu Feb 12 17:17:35 2015 +0000
+++ b/Gyroscope.cpp	Thu Feb 12 19:00:28 2015 +0000
@@ -3,40 +3,53 @@
 #include "Logger.h"
 
 Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) {
-    powerOn();
-        
-    const uint8_t chip_id = read_reg(0x00);
-    if (chip_id == 0x0f) {
+    if (powerOn()) {
         INFO("Bosch Sensortec BMX055-Gyro found");
-        deepSuspend();
     } else {
-        WARN("Bosch Sensortec BMX055-Gyro not found (chip ID=0x%02x, expected=0x0f)", chip_id);
+        WARN("Bosch Sensortec BMX055-Gyro not found");
     }
 }
 
-void Gyroscope::deepSuspend() {
+void Gyroscope::powerOff() {
     write_reg(0x11, 1);
-    LOG("deep suspend");
+    LOG("deep sleep");
 }
 
-uint32_t ticks = 0;
-frame_t frame;
-
 void Gyroscope::handleInterrupt(void) {
-    read_reg(0x3F, (uint8_t*)&frame, sizeof frame);
-    ticks++;
+    static uint32_t ticks = 0;
+    Sensor::Data frame = read();
+    
+    if (ticks % 100 == 0)
+        sendData(frame);
 }
 
-void Gyroscope::powerOn() {
+bool Gyroscope::powerOn() {
     write_reg(0x14, 0xB6); // softreset
     wait_ms(30);
-    LOG("full power mode");
+    LOG("powered on");
+    return read_reg(0x00) == 0x0f; // verify Chip ID
 }
 
-void Gyroscope::startDataCapture() {
+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)
     int1.rise(this, &Gyroscope::handleInterrupt);
+    timer.reset();
+    timer.start();
     write_reg(0x15, 1 << 7); // new data interrupt enabled
-}
\ No newline at end of file
+}
+
+void Gyroscope::stop() {
+    timer.stop();
+    write_reg(0x15, 0); // turn off new data interrupt
+}
+
+Sensor::Data Gyroscope::read() {
+    Sensor::Data frame;
+    
+    read_reg(0x3F, (uint8_t*)&frame, sizeof frame);
+    frame.timestamp = timer.read();
+    
+    return frame;
+}