Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Gyroscope.cpp@16:3e2468d4f4c1, 2015-03-20 (annotated)

Committer:
pvaibhav
Date:
Fri Mar 20 10:30:01 2015 +0000
Revision:
16:3e2468d4f4c1
Parent:
12:1632d7391453
Child:
21:5a0c9406e119
Barometer driver converted to new Sensor infrastructure

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 8:cba37530d480 5 #include "Vector3.h"
pvaibhav 8:cba37530d480 6
pvaibhav 7:604a8369b801 7 Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3)
pvaibhav 7:604a8369b801 8 {
pvaibhav 6:c12cea26842d 9 if (powerOn()) {
pvaibhav 5:b9f2f62a8f90 10 INFO("Bosch Sensortec BMX055-Gyro found");
pvaibhav 7:604a8369b801 11 powerOff();
pvaibhav 4:e759b8c756da 12 } else {
pvaibhav 6:c12cea26842d 13 WARN("Bosch Sensortec BMX055-Gyro not found");
pvaibhav 4:e759b8c756da 14 }
pvaibhav 4:e759b8c756da 15 }
pvaibhav 4:e759b8c756da 16
pvaibhav 7:604a8369b801 17 void Gyroscope::powerOff()
pvaibhav 7:604a8369b801 18 {
pvaibhav 5:b9f2f62a8f90 19 write_reg(0x11, 1);
pvaibhav 6:c12cea26842d 20 LOG("deep sleep");
pvaibhav 5:b9f2f62a8f90 21 }
pvaibhav 5:b9f2f62a8f90 22
pvaibhav 7:604a8369b801 23 void Gyroscope::handleInterrupt(void)
pvaibhav 7:604a8369b801 24 {
pvaibhav 8:cba37530d480 25 sendData(read());
pvaibhav 5:b9f2f62a8f90 26 }
pvaibhav 5:b9f2f62a8f90 27
pvaibhav 7:604a8369b801 28 bool Gyroscope::powerOn()
pvaibhav 7:604a8369b801 29 {
pvaibhav 5:b9f2f62a8f90 30 write_reg(0x14, 0xB6); // softreset
pvaibhav 5:b9f2f62a8f90 31 wait_ms(30);
pvaibhav 6:c12cea26842d 32 LOG("powered on");
pvaibhav 6:c12cea26842d 33 return read_reg(0x00) == 0x0f; // verify Chip ID
pvaibhav 5:b9f2f62a8f90 34 }
pvaibhav 5:b9f2f62a8f90 35
pvaibhav 7:604a8369b801 36 void Gyroscope::start()
pvaibhav 7:604a8369b801 37 {
pvaibhav 5:b9f2f62a8f90 38 write_reg(0x10, 5); // set capture rate: 200 Hz / filter: 23 Hz
pvaibhav 5:b9f2f62a8f90 39 write_reg(0x16, 5); // interrupts active high, push-pull
pvaibhav 5:b9f2f62a8f90 40 write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro)
pvaibhav 5:b9f2f62a8f90 41 int1.rise(this, &Gyroscope::handleInterrupt);
pvaibhav 5:b9f2f62a8f90 42 write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 6:c12cea26842d 43 }
pvaibhav 6:c12cea26842d 44
pvaibhav 7:604a8369b801 45 void Gyroscope::stop()
pvaibhav 7:604a8369b801 46 {
pvaibhav 6:c12cea26842d 47 write_reg(0x15, 0); // turn off new data interrupt
pvaibhav 6:c12cea26842d 48 }
pvaibhav 6:c12cea26842d 49
pvaibhav 8:cba37530d480 50 Vector3 Gyroscope::read()
pvaibhav 7:604a8369b801 51 {
pvaibhav 7:604a8369b801 52 // This chip causes high spikes in the data if FIFO or auto-incremented buffer is read.
pvaibhav 7:604a8369b801 53 // To work around this, we will read each register ourselves. Also keeping interrupt disabled
pvaibhav 7:604a8369b801 54 // until we finish reading all bytes.
pvaibhav 8:cba37530d480 55 const float gyro_resolution = 0.0610351563; // deg/sec
pvaibhav 8:cba37530d480 56
pvaibhav 7:604a8369b801 57 write_reg(0x15, 0); // new data interrupt disabled
pvaibhav 12:1632d7391453 58
pvaibhav 12:1632d7391453 59 uint8_t buffer[6];
pvaibhav 12:1632d7391453 60 for (size_t i = 0; i < 6; i++)
pvaibhav 12:1632d7391453 61 buffer[i] = read_reg(0x02 + i);
pvaibhav 12:1632d7391453 62
pvaibhav 12:1632d7391453 63 const int16_t x = buffer[1] << 8 | buffer[0];
pvaibhav 12:1632d7391453 64 const int16_t y = buffer[3] << 8 | buffer[2];
pvaibhav 12:1632d7391453 65 const int16_t z = buffer[5] << 8 | buffer[4];
pvaibhav 12:1632d7391453 66
pvaibhav 7:604a8369b801 67 write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 8:cba37530d480 68
pvaibhav 16:3e2468d4f4c1 69 return Vector3(x, y, z) * gyro_resolution;
pvaibhav 6:c12cea26842d 70 }