Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Gyroscope.cpp@7:604a8369b801, 2015-02-17 (annotated)

Committer:
pvaibhav
Date:
Tue Feb 17 16:53:50 2015 +0000
Revision:
7:604a8369b801
Parent:
6:c12cea26842d
Child:
8:cba37530d480
Changed reading method for sensors to avoid high spikes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pvaibhav 4:e759b8c756da 1 #include "Gyroscope.h"
pvaibhav 4:e759b8c756da 2 #define DEBUG "BMX055-Gyr"
pvaibhav 4:e759b8c756da 3 #include "Logger.h"
pvaibhav 4:e759b8c756da 4
pvaibhav 7:604a8369b801 5 Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3)
pvaibhav 7:604a8369b801 6 {
pvaibhav 6:c12cea26842d 7 if (powerOn()) {
pvaibhav 5:b9f2f62a8f90 8 INFO("Bosch Sensortec BMX055-Gyro found");
pvaibhav 7:604a8369b801 9 powerOff();
pvaibhav 4:e759b8c756da 10 } else {
pvaibhav 6:c12cea26842d 11 WARN("Bosch Sensortec BMX055-Gyro not found");
pvaibhav 4:e759b8c756da 12 }
pvaibhav 4:e759b8c756da 13 }
pvaibhav 4:e759b8c756da 14
pvaibhav 7:604a8369b801 15 void Gyroscope::powerOff()
pvaibhav 7:604a8369b801 16 {
pvaibhav 5:b9f2f62a8f90 17 write_reg(0x11, 1);
pvaibhav 6:c12cea26842d 18 LOG("deep sleep");
pvaibhav 5:b9f2f62a8f90 19 }
pvaibhav 5:b9f2f62a8f90 20
pvaibhav 7:604a8369b801 21 void Gyroscope::handleInterrupt(void)
pvaibhav 7:604a8369b801 22 {
pvaibhav 6:c12cea26842d 23 static uint32_t ticks = 0;
pvaibhav 6:c12cea26842d 24 Sensor::Data frame = read();
pvaibhav 7:604a8369b801 25
pvaibhav 6:c12cea26842d 26 if (ticks % 100 == 0)
pvaibhav 6:c12cea26842d 27 sendData(frame);
pvaibhav 5:b9f2f62a8f90 28 }
pvaibhav 5:b9f2f62a8f90 29
pvaibhav 7:604a8369b801 30 bool Gyroscope::powerOn()
pvaibhav 7:604a8369b801 31 {
pvaibhav 5:b9f2f62a8f90 32 write_reg(0x14, 0xB6); // softreset
pvaibhav 5:b9f2f62a8f90 33 wait_ms(30);
pvaibhav 6:c12cea26842d 34 LOG("powered on");
pvaibhav 6:c12cea26842d 35 return read_reg(0x00) == 0x0f; // verify Chip ID
pvaibhav 5:b9f2f62a8f90 36 }
pvaibhav 5:b9f2f62a8f90 37
pvaibhav 7:604a8369b801 38 void Gyroscope::start()
pvaibhav 7:604a8369b801 39 {
pvaibhav 5:b9f2f62a8f90 40 write_reg(0x10, 5); // set capture rate: 200 Hz / filter: 23 Hz
pvaibhav 5:b9f2f62a8f90 41 write_reg(0x16, 5); // interrupts active high, push-pull
pvaibhav 5:b9f2f62a8f90 42 write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro)
pvaibhav 7:604a8369b801 43 //write_reg(0x18, 1 << 2); // map fifo interrupt to INT3 pin
pvaibhav 5:b9f2f62a8f90 44 int1.rise(this, &Gyroscope::handleInterrupt);
pvaibhav 7:604a8369b801 45 timer.stop();
pvaibhav 6:c12cea26842d 46 timer.reset();
pvaibhav 6:c12cea26842d 47 timer.start();
pvaibhav 5:b9f2f62a8f90 48 write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 7:604a8369b801 49 //write_reg(0x15, 1 << 6); // fifo interrupt enabled
pvaibhav 6:c12cea26842d 50 }
pvaibhav 6:c12cea26842d 51
pvaibhav 7:604a8369b801 52 void Gyroscope::stop()
pvaibhav 7:604a8369b801 53 {
pvaibhav 6:c12cea26842d 54 timer.stop();
pvaibhav 6:c12cea26842d 55 write_reg(0x15, 0); // turn off new data interrupt
pvaibhav 6:c12cea26842d 56 }
pvaibhav 6:c12cea26842d 57
pvaibhav 7:604a8369b801 58 Sensor::Data Gyroscope::read()
pvaibhav 7:604a8369b801 59 {
pvaibhav 6:c12cea26842d 60 Sensor::Data frame;
pvaibhav 6:c12cea26842d 61
pvaibhav 7:604a8369b801 62 // This chip causes high spikes in the data if FIFO or auto-incremented buffer is read.
pvaibhav 7:604a8369b801 63 // To work around this, we will read each register ourselves. Also keeping interrupt disabled
pvaibhav 7:604a8369b801 64 // until we finish reading all bytes.
pvaibhav 7:604a8369b801 65
pvaibhav 7:604a8369b801 66 write_reg(0x15, 0); // new data interrupt disabled
pvaibhav 7:604a8369b801 67
pvaibhav 7:604a8369b801 68 frame.x = read_reg(0x02) | (read_reg(0x03) << 8);
pvaibhav 7:604a8369b801 69 frame.y = read_reg(0x04) | (read_reg(0x05) << 8);
pvaibhav 7:604a8369b801 70 frame.z = read_reg(0x06) | (read_reg(0x07) << 8);
pvaibhav 7:604a8369b801 71
pvaibhav 7:604a8369b801 72 write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 6:c12cea26842d 73
pvaibhav 6:c12cea26842d 74 return frame;
pvaibhav 6:c12cea26842d 75 }