Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Gyroscope.cpp@39:1fa9c0e1ffde, 2015-05-19 (annotated)

Committer:
pvaibhav
Date:
Tue May 19 14:18:30 2015 +0000
Revision:
39:1fa9c0e1ffde
Parent:
32:d37447aec6b4
Child:
40:8e852115fe55
sixaxis

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 39:1fa9c0e1ffde 7 Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3), int2(p1), tick(0)
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 21:5a0c9406e119 25 delegate->sensorUpdate(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 25:abb0f208e6a9 38 write_reg(0x10, 5); // set capture rate: 100 Hz / filter: 12 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 39:1fa9c0e1ffde 42 tick = 1;
pvaibhav 5:b9f2f62a8f90 43 write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 6:c12cea26842d 44 }
pvaibhav 6:c12cea26842d 45
pvaibhav 7:604a8369b801 46 void Gyroscope::stop()
pvaibhav 7:604a8369b801 47 {
pvaibhav 6:c12cea26842d 48 write_reg(0x15, 0); // turn off new data interrupt
pvaibhav 6:c12cea26842d 49 }
pvaibhav 6:c12cea26842d 50
pvaibhav 8:cba37530d480 51 Vector3 Gyroscope::read()
pvaibhav 7:604a8369b801 52 {
pvaibhav 7:604a8369b801 53 // This chip causes high spikes in the data if FIFO or auto-incremented buffer is read.
pvaibhav 7:604a8369b801 54 // To work around this, we will read each register ourselves. Also keeping interrupt disabled
pvaibhav 7:604a8369b801 55 // until we finish reading all bytes.
pvaibhav 8:cba37530d480 56 const float gyro_resolution = 0.0610351563; // deg/sec
pvaibhav 8:cba37530d480 57
pvaibhav 31:d65576185bdf 58 //write_reg(0x15, 0); // new data interrupt disabled
pvaibhav 12:1632d7391453 59
pvaibhav 12:1632d7391453 60 uint8_t buffer[6];
pvaibhav 12:1632d7391453 61 for (size_t i = 0; i < 6; i++)
pvaibhav 12:1632d7391453 62 buffer[i] = read_reg(0x02 + i);
pvaibhav 12:1632d7391453 63
pvaibhav 12:1632d7391453 64 const int16_t x = buffer[1] << 8 | buffer[0];
pvaibhav 12:1632d7391453 65 const int16_t y = buffer[3] << 8 | buffer[2];
pvaibhav 12:1632d7391453 66 const int16_t z = buffer[5] << 8 | buffer[4];
pvaibhav 12:1632d7391453 67
pvaibhav 31:d65576185bdf 68 //write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 8:cba37530d480 69
pvaibhav 16:3e2468d4f4c1 70 return Vector3(x, y, z) * gyro_resolution;
pvaibhav 6:c12cea26842d 71 }