Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Gyroscope.cpp@5:b9f2f62a8f90, 2015-02-12 (annotated)
- Committer:
- pvaibhav
- Date:
- Thu Feb 12 17:17:35 2015 +0000
- Revision:
- 5:b9f2f62a8f90
- Parent:
- 4:e759b8c756da
- Child:
- 6:c12cea26842d
Gyro data can now be read fine. 100 Hz.
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 | 5:b9f2f62a8f90 | 5 | Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) { |
pvaibhav | 5:b9f2f62a8f90 | 6 | powerOn(); |
pvaibhav | 5:b9f2f62a8f90 | 7 | |
pvaibhav | 4:e759b8c756da | 8 | const uint8_t chip_id = read_reg(0x00); |
pvaibhav | 4:e759b8c756da | 9 | if (chip_id == 0x0f) { |
pvaibhav | 5:b9f2f62a8f90 | 10 | INFO("Bosch Sensortec BMX055-Gyro found"); |
pvaibhav | 5:b9f2f62a8f90 | 11 | deepSuspend(); |
pvaibhav | 4:e759b8c756da | 12 | } else { |
pvaibhav | 4:e759b8c756da | 13 | WARN("Bosch Sensortec BMX055-Gyro not found (chip ID=0x%02x, expected=0x0f)", chip_id); |
pvaibhav | 4:e759b8c756da | 14 | } |
pvaibhav | 4:e759b8c756da | 15 | } |
pvaibhav | 4:e759b8c756da | 16 | |
pvaibhav | 5:b9f2f62a8f90 | 17 | void Gyroscope::deepSuspend() { |
pvaibhav | 5:b9f2f62a8f90 | 18 | write_reg(0x11, 1); |
pvaibhav | 5:b9f2f62a8f90 | 19 | LOG("deep suspend"); |
pvaibhav | 5:b9f2f62a8f90 | 20 | } |
pvaibhav | 5:b9f2f62a8f90 | 21 | |
pvaibhav | 5:b9f2f62a8f90 | 22 | uint32_t ticks = 0; |
pvaibhav | 5:b9f2f62a8f90 | 23 | frame_t frame; |
pvaibhav | 5:b9f2f62a8f90 | 24 | |
pvaibhav | 5:b9f2f62a8f90 | 25 | void Gyroscope::handleInterrupt(void) { |
pvaibhav | 5:b9f2f62a8f90 | 26 | read_reg(0x3F, (uint8_t*)&frame, sizeof frame); |
pvaibhav | 5:b9f2f62a8f90 | 27 | ticks++; |
pvaibhav | 5:b9f2f62a8f90 | 28 | } |
pvaibhav | 5:b9f2f62a8f90 | 29 | |
pvaibhav | 5:b9f2f62a8f90 | 30 | void Gyroscope::powerOn() { |
pvaibhav | 5:b9f2f62a8f90 | 31 | write_reg(0x14, 0xB6); // softreset |
pvaibhav | 5:b9f2f62a8f90 | 32 | wait_ms(30); |
pvaibhav | 5:b9f2f62a8f90 | 33 | LOG("full power mode"); |
pvaibhav | 5:b9f2f62a8f90 | 34 | } |
pvaibhav | 5:b9f2f62a8f90 | 35 | |
pvaibhav | 5:b9f2f62a8f90 | 36 | void Gyroscope::startDataCapture() { |
pvaibhav | 5:b9f2f62a8f90 | 37 | write_reg(0x10, 5); // set capture rate: 200 Hz / filter: 23 Hz |
pvaibhav | 5:b9f2f62a8f90 | 38 | write_reg(0x16, 5); // interrupts active high, push-pull |
pvaibhav | 5:b9f2f62a8f90 | 39 | write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro) |
pvaibhav | 5:b9f2f62a8f90 | 40 | int1.rise(this, &Gyroscope::handleInterrupt); |
pvaibhav | 5:b9f2f62a8f90 | 41 | write_reg(0x15, 1 << 7); // new data interrupt enabled |
pvaibhav | 5:b9f2f62a8f90 | 42 | } |