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