Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Gyroscope.cpp@7:604a8369b801, 2015-02-17 (annotated)
- Committer:
- pvaibhav
- Date:
- Tue Feb 17 16:53:50 2015 +0000
- Revision:
- 7:604a8369b801
- Parent:
- 6:c12cea26842d
- Child:
- 8:cba37530d480
Changed reading method for sensors to avoid high spikes
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 | 7:604a8369b801 | 5 | Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) |
pvaibhav | 7:604a8369b801 | 6 | { |
pvaibhav | 6:c12cea26842d | 7 | if (powerOn()) { |
pvaibhav | 5:b9f2f62a8f90 | 8 | INFO("Bosch Sensortec BMX055-Gyro found"); |
pvaibhav | 7:604a8369b801 | 9 | powerOff(); |
pvaibhav | 4:e759b8c756da | 10 | } else { |
pvaibhav | 6:c12cea26842d | 11 | WARN("Bosch Sensortec BMX055-Gyro not found"); |
pvaibhav | 4:e759b8c756da | 12 | } |
pvaibhav | 4:e759b8c756da | 13 | } |
pvaibhav | 4:e759b8c756da | 14 | |
pvaibhav | 7:604a8369b801 | 15 | void Gyroscope::powerOff() |
pvaibhav | 7:604a8369b801 | 16 | { |
pvaibhav | 5:b9f2f62a8f90 | 17 | write_reg(0x11, 1); |
pvaibhav | 6:c12cea26842d | 18 | LOG("deep sleep"); |
pvaibhav | 5:b9f2f62a8f90 | 19 | } |
pvaibhav | 5:b9f2f62a8f90 | 20 | |
pvaibhav | 7:604a8369b801 | 21 | void Gyroscope::handleInterrupt(void) |
pvaibhav | 7:604a8369b801 | 22 | { |
pvaibhav | 6:c12cea26842d | 23 | static uint32_t ticks = 0; |
pvaibhav | 6:c12cea26842d | 24 | Sensor::Data frame = read(); |
pvaibhav | 7:604a8369b801 | 25 | |
pvaibhav | 6:c12cea26842d | 26 | if (ticks % 100 == 0) |
pvaibhav | 6:c12cea26842d | 27 | sendData(frame); |
pvaibhav | 5:b9f2f62a8f90 | 28 | } |
pvaibhav | 5:b9f2f62a8f90 | 29 | |
pvaibhav | 7:604a8369b801 | 30 | bool Gyroscope::powerOn() |
pvaibhav | 7:604a8369b801 | 31 | { |
pvaibhav | 5:b9f2f62a8f90 | 32 | write_reg(0x14, 0xB6); // softreset |
pvaibhav | 5:b9f2f62a8f90 | 33 | wait_ms(30); |
pvaibhav | 6:c12cea26842d | 34 | LOG("powered on"); |
pvaibhav | 6:c12cea26842d | 35 | return read_reg(0x00) == 0x0f; // verify Chip ID |
pvaibhav | 5:b9f2f62a8f90 | 36 | } |
pvaibhav | 5:b9f2f62a8f90 | 37 | |
pvaibhav | 7:604a8369b801 | 38 | void Gyroscope::start() |
pvaibhav | 7:604a8369b801 | 39 | { |
pvaibhav | 5:b9f2f62a8f90 | 40 | write_reg(0x10, 5); // set capture rate: 200 Hz / filter: 23 Hz |
pvaibhav | 5:b9f2f62a8f90 | 41 | write_reg(0x16, 5); // interrupts active high, push-pull |
pvaibhav | 5:b9f2f62a8f90 | 42 | write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro) |
pvaibhav | 7:604a8369b801 | 43 | //write_reg(0x18, 1 << 2); // map fifo interrupt to INT3 pin |
pvaibhav | 5:b9f2f62a8f90 | 44 | int1.rise(this, &Gyroscope::handleInterrupt); |
pvaibhav | 7:604a8369b801 | 45 | timer.stop(); |
pvaibhav | 6:c12cea26842d | 46 | timer.reset(); |
pvaibhav | 6:c12cea26842d | 47 | timer.start(); |
pvaibhav | 5:b9f2f62a8f90 | 48 | write_reg(0x15, 1 << 7); // new data interrupt enabled |
pvaibhav | 7:604a8369b801 | 49 | //write_reg(0x15, 1 << 6); // fifo interrupt enabled |
pvaibhav | 6:c12cea26842d | 50 | } |
pvaibhav | 6:c12cea26842d | 51 | |
pvaibhav | 7:604a8369b801 | 52 | void Gyroscope::stop() |
pvaibhav | 7:604a8369b801 | 53 | { |
pvaibhav | 6:c12cea26842d | 54 | timer.stop(); |
pvaibhav | 6:c12cea26842d | 55 | write_reg(0x15, 0); // turn off new data interrupt |
pvaibhav | 6:c12cea26842d | 56 | } |
pvaibhav | 6:c12cea26842d | 57 | |
pvaibhav | 7:604a8369b801 | 58 | Sensor::Data Gyroscope::read() |
pvaibhav | 7:604a8369b801 | 59 | { |
pvaibhav | 6:c12cea26842d | 60 | Sensor::Data frame; |
pvaibhav | 6:c12cea26842d | 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 | 7:604a8369b801 | 65 | |
pvaibhav | 7:604a8369b801 | 66 | write_reg(0x15, 0); // new data interrupt disabled |
pvaibhav | 7:604a8369b801 | 67 | |
pvaibhav | 7:604a8369b801 | 68 | frame.x = read_reg(0x02) | (read_reg(0x03) << 8); |
pvaibhav | 7:604a8369b801 | 69 | frame.y = read_reg(0x04) | (read_reg(0x05) << 8); |
pvaibhav | 7:604a8369b801 | 70 | frame.z = read_reg(0x06) | (read_reg(0x07) << 8); |
pvaibhav | 7:604a8369b801 | 71 | |
pvaibhav | 7:604a8369b801 | 72 | write_reg(0x15, 1 << 7); // new data interrupt enabled |
pvaibhav | 6:c12cea26842d | 73 | |
pvaibhav | 6:c12cea26842d | 74 | return frame; |
pvaibhav | 6:c12cea26842d | 75 | } |