File content as of revision 31:d65576185bdf:

#include "Gyroscope.h"
#define DEBUG "BMX055-Gyr"
#include "Logger.h"

#include "Vector3.h"

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()
{
    write_reg(0x11, 1);
    LOG("deep sleep");
}

void Gyroscope::handleInterrupt(void)
{
    delegate->sensorUpdate(read());
}

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()
{
    write_reg(0x10, 5); // set capture rate: 100 Hz / filter: 12 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
}

void Gyroscope::stop()
{
    write_reg(0x15, 0); // turn off new data interrupt
}

Vector3 Gyroscope::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.
    const float gyro_resolution = 0.0610351563; // deg/sec

    //write_reg(0x15, 0); // new data interrupt disabled

    uint8_t buffer[6];
    for (size_t i = 0; i < 6; i++)
        buffer[i] = read_reg(0x02 + i);

    const int16_t x = buffer[1] << 8 | buffer[0];
    const int16_t y = buffer[3] << 8 | buffer[2];
    const int16_t z = buffer[5] << 8 | buffer[4];

    //write_reg(0x15, 1 << 7); // new data interrupt enabled

    return Vector3(x, y, z) * gyro_resolution;
}