Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Gyroscope.cpp

Committer:
pvaibhav
Date:
2015-02-17
Revision:
7:604a8369b801
Parent:
6:c12cea26842d
Child:
8:cba37530d480

File content as of revision 7:604a8369b801:

#include "Gyroscope.h"
#define DEBUG "BMX055-Gyr"
#include "Logger.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)
{
    static uint32_t ticks = 0;
    Sensor::Data frame = read();

    if (ticks % 100 == 0)
        sendData(frame);
}

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)
    //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()
{
    timer.stop();
    write_reg(0x15, 0); // turn off new data interrupt
}

Sensor::Data Gyroscope::read()
{
    Sensor::Data frame;
    
    // 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;
}