Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Gyroscope.cpp
- Revision:
- 5:b9f2f62a8f90
- Parent:
- 4:e759b8c756da
- Child:
- 6:c12cea26842d
--- a/Gyroscope.cpp Fri Jan 23 13:00:46 2015 +0000 +++ b/Gyroscope.cpp Thu Feb 12 17:17:35 2015 +0000 @@ -2,15 +2,41 @@ #define DEBUG "BMX055-Gyr" #include "Logger.h" -Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */) { - write_reg(0x14, 0xB6); // reset - wait_ms(30); // page 14 - +Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) { + powerOn(); + const uint8_t chip_id = read_reg(0x00); if (chip_id == 0x0f) { - INFO("Bosch Sensortec BMX055-Gyro ready"); + INFO("Bosch Sensortec BMX055-Gyro found"); + deepSuspend(); } else { WARN("Bosch Sensortec BMX055-Gyro not found (chip ID=0x%02x, expected=0x0f)", chip_id); } } +void Gyroscope::deepSuspend() { + write_reg(0x11, 1); + LOG("deep suspend"); +} + +uint32_t ticks = 0; +frame_t frame; + +void Gyroscope::handleInterrupt(void) { + read_reg(0x3F, (uint8_t*)&frame, sizeof frame); + ticks++; +} + +void Gyroscope::powerOn() { + write_reg(0x14, 0xB6); // softreset + wait_ms(30); + LOG("full power mode"); +} + +void Gyroscope::startDataCapture() { + write_reg(0x10, 5); // set capture rate: 200 Hz / filter: 23 Hz + write_reg(0x16, 5); // interrupts active high, push-pull + write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro) + int1.rise(this, &Gyroscope::handleInterrupt); + write_reg(0x15, 1 << 7); // new data interrupt enabled +} \ No newline at end of file