File content as of revision 11:d21275e60ebb:

#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)
{
    sendData(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: 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
}

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
    
    union {
        uint8_t bytes[6];
        struct {
            int16_t z;
            int16_t y;
            int16_t x;
        } val;
    } buffer;
    
    size_t i = sizeof buffer;
    while (i --> 0) {
        buffer.bytes[i] = read_reg(0x02 + i);
    };
    
    write_reg(0x15, 1 << 7); // new data interrupt enabled

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