Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Gyroscope.cpp@32:d37447aec6b4, 2015-05-04 (annotated)

Committer:
pvaibhav
Date:
Mon May 04 15:16:57 2015 +0000
Revision:
32:d37447aec6b4
Parent:
31:d65576185bdf
Child:
39:1fa9c0e1ffde
Interrupt lines added to all sensors and motor driver, power aware I2C subclass added (currently doesn't do anything)

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 32:d37447aec6b4 7 Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3), int2(p1)
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 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 31:d65576185bdf 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 31:d65576185bdf 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 }