Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Gyroscope.cpp@44:5483079fa156, 2015-05-29 (annotated)

Committer:
uadhikari
Date:
Fri May 29 12:21:31 2015 +0000
Revision:
44:5483079fa156
Parent:
40:8e852115fe55 
Gyro calibration and reset added and flag to check if calibrated

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
uadhikari 44:5483079fa156 7 Gyroscope::Gyroscope(I2C &i2c) :
uadhikari 44:5483079fa156 8 I2CPeripheral(i2c, 0x69 << 1 /* address */),
uadhikari 44:5483079fa156 9 isCalibrated(false),
uadhikari 44:5483079fa156 10 interruptSet(false),
uadhikari 44:5483079fa156 11 numSamples(0),
uadhikari 44:5483079fa156 12 offset(0.0, 0.0, 0.0),
uadhikari 44:5483079fa156 13 int1(p3),
uadhikari 44:5483079fa156 14 int2(p1),
uadhikari 44:5483079fa156 15 tick(0)
pvaibhav 7:604a8369b801 16 {
pvaibhav 6:c12cea26842d 17 if (powerOn()) {
pvaibhav 5:b9f2f62a8f90 18 INFO("Bosch Sensortec BMX055-Gyro found");
pvaibhav 7:604a8369b801 19 powerOff();
pvaibhav 4:e759b8c756da 20 } else {
pvaibhav 6:c12cea26842d 21 WARN("Bosch Sensortec BMX055-Gyro not found");
pvaibhav 4:e759b8c756da 22 }
pvaibhav 4:e759b8c756da 23 }
pvaibhav 4:e759b8c756da 24
pvaibhav 7:604a8369b801 25 void Gyroscope::powerOff()
pvaibhav 7:604a8369b801 26 {
pvaibhav 5:b9f2f62a8f90 27 write_reg(0x11, 1);
pvaibhav 6:c12cea26842d 28 LOG("deep sleep");
pvaibhav 5:b9f2f62a8f90 29 }
pvaibhav 5:b9f2f62a8f90 30
pvaibhav 7:604a8369b801 31 void Gyroscope::handleInterrupt(void)
pvaibhav 7:604a8369b801 32 {
pvaibhav 21:5a0c9406e119 33 delegate->sensorUpdate(read());
pvaibhav 5:b9f2f62a8f90 34 }
pvaibhav 5:b9f2f62a8f90 35
pvaibhav 7:604a8369b801 36 bool Gyroscope::powerOn()
pvaibhav 7:604a8369b801 37 {
pvaibhav 5:b9f2f62a8f90 38 write_reg(0x14, 0xB6); // softreset
pvaibhav 5:b9f2f62a8f90 39 wait_ms(30);
pvaibhav 6:c12cea26842d 40 LOG("powered on");
pvaibhav 6:c12cea26842d 41 return read_reg(0x00) == 0x0f; // verify Chip ID
pvaibhav 5:b9f2f62a8f90 42 }
pvaibhav 5:b9f2f62a8f90 43
pvaibhav 7:604a8369b801 44 void Gyroscope::start()
pvaibhav 7:604a8369b801 45 {
pvaibhav 25:abb0f208e6a9 46 write_reg(0x10, 5); // set capture rate: 100 Hz / filter: 12 Hz
pvaibhav 40:8e852115fe55 47 write_reg(0x0F, 2); // set range +/- 500 deg/sec
pvaibhav 5:b9f2f62a8f90 48 write_reg(0x16, 5); // interrupts active high, push-pull
pvaibhav 5:b9f2f62a8f90 49 write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro)
pvaibhav 5:b9f2f62a8f90 50 int1.rise(this, &Gyroscope::handleInterrupt);
pvaibhav 39:1fa9c0e1ffde 51 tick = 1;
pvaibhav 5:b9f2f62a8f90 52 write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 6:c12cea26842d 53 }
pvaibhav 6:c12cea26842d 54
pvaibhav 7:604a8369b801 55 void Gyroscope::stop()
pvaibhav 7:604a8369b801 56 {
pvaibhav 6:c12cea26842d 57 write_reg(0x15, 0); // turn off new data interrupt
pvaibhav 6:c12cea26842d 58 }
pvaibhav 6:c12cea26842d 59
uadhikari 44:5483079fa156 60 Vector3 Gyroscope::uncalibrated_read()
pvaibhav 7:604a8369b801 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 40:8e852115fe55 65 //const float gyro_resolution = 0.0610351563; // deg/sec for 2000 deg/sec range
pvaibhav 8:cba37530d480 66
pvaibhav 31:d65576185bdf 67 //write_reg(0x15, 0); // new data interrupt disabled
pvaibhav 12:1632d7391453 68
pvaibhav 12:1632d7391453 69 uint8_t buffer[6];
pvaibhav 40:8e852115fe55 70 /*
pvaibhav 12:1632d7391453 71 for (size_t i = 0; i < 6; i++)
pvaibhav 12:1632d7391453 72 buffer[i] = read_reg(0x02 + i);
pvaibhav 40:8e852115fe55 73 */
pvaibhav 40:8e852115fe55 74
pvaibhav 40:8e852115fe55 75 read_reg(0x02, buffer, sizeof buffer);
pvaibhav 40:8e852115fe55 76
pvaibhav 12:1632d7391453 77 const int16_t x = buffer[1] << 8 | buffer[0];
pvaibhav 12:1632d7391453 78 const int16_t y = buffer[3] << 8 | buffer[2];
pvaibhav 12:1632d7391453 79 const int16_t z = buffer[5] << 8 | buffer[4];
pvaibhav 12:1632d7391453 80
uadhikari 44:5483079fa156 81 //write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 40:8e852115fe55 82 //static Vector3 const offsets(-0.16, -0.17, 0.015);
pvaibhav 8:cba37530d480 83
uadhikari 44:5483079fa156 84 return Vector3(x, y, z);
uadhikari 44:5483079fa156 85 }
uadhikari 44:5483079fa156 86
uadhikari 44:5483079fa156 87 Vector3 Gyroscope::read()
uadhikari 44:5483079fa156 88 {
uadhikari 44:5483079fa156 89 const float gyro_resolution = 0.0152587891;
uadhikari 44:5483079fa156 90 return (uncalibrated_read() - offset) * gyro_resolution;
pvaibhav 6:c12cea26842d 91 }
uadhikari 44:5483079fa156 92
uadhikari 44:5483079fa156 93 //http://math.stackexchange.com/questions/106700/incremental-averageing
uadhikari 44:5483079fa156 94 void Gyroscope::calibtrationInterrupt(void){
uadhikari 44:5483079fa156 95 const static size_t maxNumCalibrationSamples = 200;
uadhikari 44:5483079fa156 96
uadhikari 44:5483079fa156 97 Vector3 rawGyroReading = uncalibrated_read();
uadhikari 44:5483079fa156 98
uadhikari 44:5483079fa156 99 //newMean = oldMean + (currentReading - oldMean) / newNumSamples;
uadhikari 44:5483079fa156 100 offset = offset + (rawGyroReading - offset) / (++numSamples);
uadhikari 44:5483079fa156 101
uadhikari 44:5483079fa156 102 //Turn off calibration once we read desired number of samples
uadhikari 44:5483079fa156 103 if(numSamples == maxNumCalibrationSamples){
uadhikari 44:5483079fa156 104 numSamples = 0;
uadhikari 44:5483079fa156 105 isCalibrated = true;
uadhikari 44:5483079fa156 106 int1.rise(this, &Gyroscope::handleInterrupt);
uadhikari 44:5483079fa156 107 }
uadhikari 44:5483079fa156 108 }
uadhikari 44:5483079fa156 109
uadhikari 44:5483079fa156 110 void Gyroscope::calibrate(){
uadhikari 44:5483079fa156 111 int1.rise(this, &Gyroscope::calibtrationInterrupt);
uadhikari 44:5483079fa156 112 }
uadhikari 44:5483079fa156 113
uadhikari 44:5483079fa156 114 void Gyroscope::reset(){
uadhikari 44:5483079fa156 115 offset.x = 0.0;
uadhikari 44:5483079fa156 116 offset.y = 0.0;
uadhikari 44:5483079fa156 117 offset.z = 0.0;
uadhikari 44:5483079fa156 118
uadhikari 44:5483079fa156 119 }