Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Gyroscope.cpp@46:fd5a62296b12, 2015-05-27 (annotated)

Committer:
pvaibhav
Date:
Wed May 27 13:01:43 2015 +0000
Revision:
46:fd5a62296b12
Parent:
40:8e852115fe55 
Code reformatted

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 40:8e852115fe55 7 Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), interruptSet(false), 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 40:8e852115fe55 39 write_reg(0x0F, 2); // set range +/- 500 deg/sec
pvaibhav 5:b9f2f62a8f90 40 write_reg(0x16, 5); // interrupts active high, push-pull
pvaibhav 5:b9f2f62a8f90 41 write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro)
pvaibhav 5:b9f2f62a8f90 42 int1.rise(this, &Gyroscope::handleInterrupt);
pvaibhav 39:1fa9c0e1ffde 43 tick = 1;
pvaibhav 5:b9f2f62a8f90 44 write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 6:c12cea26842d 45 }
pvaibhav 6:c12cea26842d 46
pvaibhav 7:604a8369b801 47 void Gyroscope::stop()
pvaibhav 7:604a8369b801 48 {
pvaibhav 6:c12cea26842d 49 write_reg(0x15, 0); // turn off new data interrupt
pvaibhav 6:c12cea26842d 50 }
pvaibhav 6:c12cea26842d 51
pvaibhav 8:cba37530d480 52 Vector3 Gyroscope::read()
pvaibhav 7:604a8369b801 53 {
pvaibhav 7:604a8369b801 54 // This chip causes high spikes in the data if FIFO or auto-incremented buffer is read.
pvaibhav 7:604a8369b801 55 // To work around this, we will read each register ourselves. Also keeping interrupt disabled
pvaibhav 7:604a8369b801 56 // until we finish reading all bytes.
pvaibhav 40:8e852115fe55 57 //const float gyro_resolution = 0.0610351563; // deg/sec for 2000 deg/sec range
pvaibhav 40:8e852115fe55 58 const float gyro_resolution = 0.0152587891; // deg/sec for 500 deg/sec range
pvaibhav 8:cba37530d480 59
pvaibhav 31:d65576185bdf 60 //write_reg(0x15, 0); // new data interrupt disabled
pvaibhav 12:1632d7391453 61
pvaibhav 12:1632d7391453 62 uint8_t buffer[6];
pvaibhav 40:8e852115fe55 63 /*
pvaibhav 12:1632d7391453 64 for (size_t i = 0; i < 6; i++)
pvaibhav 12:1632d7391453 65 buffer[i] = read_reg(0x02 + i);
pvaibhav 40:8e852115fe55 66 */
pvaibhav 46:fd5a62296b12 67
pvaibhav 40:8e852115fe55 68 read_reg(0x02, buffer, sizeof buffer);
pvaibhav 46:fd5a62296b12 69
pvaibhav 12:1632d7391453 70 const int16_t x = buffer[1] << 8 | buffer[0];
pvaibhav 12:1632d7391453 71 const int16_t y = buffer[3] << 8 | buffer[2];
pvaibhav 12:1632d7391453 72 const int16_t z = buffer[5] << 8 | buffer[4];
pvaibhav 12:1632d7391453 73
pvaibhav 31:d65576185bdf 74 //write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 46:fd5a62296b12 75
pvaibhav 40:8e852115fe55 76 //static Vector3 const offsets(-0.16, -0.17, 0.015);
pvaibhav 8:cba37530d480 77
pvaibhav 40:8e852115fe55 78 return (Vector3(x, y, z) * gyro_resolution);// - offsets;
pvaibhav 6:c12cea26842d 79 }