Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
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?
User | Revision | Line number | New 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 | } |