Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Gyroscope.cpp
- Revision:
- 7:604a8369b801
- Parent:
- 6:c12cea26842d
- Child:
- 8:cba37530d480
diff -r c12cea26842d -r 604a8369b801 Gyroscope.cpp --- a/Gyroscope.cpp Thu Feb 12 19:00:28 2015 +0000 +++ b/Gyroscope.cpp Tue Feb 17 16:53:50 2015 +0000 @@ -2,54 +2,74 @@ #define DEBUG "BMX055-Gyr" #include "Logger.h" -Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) { +Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) +{ if (powerOn()) { INFO("Bosch Sensortec BMX055-Gyro found"); + powerOff(); } else { WARN("Bosch Sensortec BMX055-Gyro not found"); } } -void Gyroscope::powerOff() { +void Gyroscope::powerOff() +{ write_reg(0x11, 1); LOG("deep sleep"); } -void Gyroscope::handleInterrupt(void) { +void Gyroscope::handleInterrupt(void) +{ static uint32_t ticks = 0; Sensor::Data frame = read(); - + if (ticks % 100 == 0) sendData(frame); } -bool Gyroscope::powerOn() { +bool Gyroscope::powerOn() +{ write_reg(0x14, 0xB6); // softreset wait_ms(30); LOG("powered on"); return read_reg(0x00) == 0x0f; // verify Chip ID } -void Gyroscope::start() { +void Gyroscope::start() +{ 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) + //write_reg(0x18, 1 << 2); // map fifo interrupt to INT3 pin int1.rise(this, &Gyroscope::handleInterrupt); + timer.stop(); timer.reset(); timer.start(); write_reg(0x15, 1 << 7); // new data interrupt enabled + //write_reg(0x15, 1 << 6); // fifo interrupt enabled } -void Gyroscope::stop() { +void Gyroscope::stop() +{ timer.stop(); write_reg(0x15, 0); // turn off new data interrupt } -Sensor::Data Gyroscope::read() { +Sensor::Data Gyroscope::read() +{ Sensor::Data frame; - read_reg(0x3F, (uint8_t*)&frame, sizeof frame); - frame.timestamp = timer.read(); + // This chip causes high spikes in the data if FIFO or auto-incremented buffer is read. + // To work around this, we will read each register ourselves. Also keeping interrupt disabled + // until we finish reading all bytes. + + write_reg(0x15, 0); // new data interrupt disabled + + frame.x = read_reg(0x02) | (read_reg(0x03) << 8); + frame.y = read_reg(0x04) | (read_reg(0x05) << 8); + frame.z = read_reg(0x06) | (read_reg(0x07) << 8); + + write_reg(0x15, 1 << 7); // new data interrupt enabled return frame; }