File content as of revision 15:e61fe4b74daa:

/**
 * @file ITG3200.cpp
 * @author Tyler Weaver
 *
 * @section LICENSE
 *
 * Copyright (c) 2010 ARM Limited
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * @section DESCRIPTION
 *
 * Modified version of Aaron Berk's library.
 * ITG-3200 triple axis, digital interface, gyroscope.
 *
 * Datasheet:
 *
 * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
 */

#include "ITG3200.h"
#include <new>

ITG3200::ITG3200(PinName sda, PinName scl, bool fastmode) : calibSamples(0), i2c_(*reinterpret_cast<I2C*>(i2cRaw))
{
    // Placement new to avoid additional heap memory allocation.
    new(i2cRaw) I2C(sda, scl);

    offset[0] = offset[1] = offset[2] = 0.0;

    if(fastmode) {
        //400kHz, fast mode.
        i2c_.frequency(400000);
    } else
        i2c_.frequency(100000);
}

ITG3200::~ITG3200()
{
    // If the I2C object is initialized in the buffer in this object, call destructor of it.
    if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
        reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
}

void ITG3200::init()
{
    //Set FS_SEL to 0x03 for proper operation.
    //See datasheet for details.
    char tx[2];
    
    // Power up reset defaults
    tx[0] = 0x3E; // Power management
    tx[1] = 0x80; // ?
    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
    wait_ms(5);
    
    // Select full-scale range of the gyro sensors
    // Set LP filter bandwidth to 42Hz
    tx[0] = 0x16; //
    tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
    wait_ms(5);
    
    // set sample rate to 50 Hz
    tx[0] = 0x15; //
    tx[1] = 0x0A; //  SMPLRT_DIV = 10 (50Hz)
    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
    wait_ms(5);
    
    // Set clock to PLL with z gyro reference
    tx[0] = 0x3E;
    tx[1] = 0x00;
    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
    wait_ms(5);
    
    const float offset[3] = {99.5, -45.0, -29.7}; // taken from itg3200.xls curve fit test
    const float slope[3] = {-1.05, 0.95, 0.47};

    setCalibrationCurve(offset, slope);
}

void ITG3200::setCalibrationCurve(const float offset[3], const float slope[3])
{
    if(offset) {
        for(int i = 0; i < 3; i++)
            this->foffset[i] = offset[i];
    }
    if(slope) {
        for(int i = 0; i < 3; i++)
            this->slope[i] = slope[i];
    }
}

char ITG3200::getWhoAmI(void)
{

    //WhoAmI Register address.
    char tx = WHO_AM_I_REG;
    char rx;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);

    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);

    return rx;

}

void ITG3200::setWhoAmI(char address)
{

    char tx[2];
    tx[0] = WHO_AM_I_REG;
    tx[1] = address;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);

}

char ITG3200::getSampleRateDivider(void)
{

    char tx = SMPLRT_DIV_REG;
    char rx;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);

    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);

    return rx;

}

void ITG3200::setSampleRateDivider(char divider)
{

    char tx[2];
    tx[0] = SMPLRT_DIV_REG;
    tx[1] = divider;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);

}

int ITG3200::getInternalSampleRate(void)
{

    char tx = DLPF_FS_REG;
    char rx;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);

    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);

    //DLPF_CFG == 0 -> sample rate = 8kHz.
    if(rx == 0) {
        return 8;
    }
    //DLPF_CFG = 1..7 -> sample rate = 1kHz.
    else if(rx >= 1 && rx <= 7) {
        return 1;
    }
    //DLPF_CFG = anything else -> something's wrong!
    else {
        return -1;
    }

}

void ITG3200::setLpBandwidth(char bandwidth)
{

    char tx[2];
    tx[0] = DLPF_FS_REG;
    //Bits 4,3 are required to be 0x03 for proper operation.
    tx[1] = bandwidth | (0x03 << 3);

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);

}

char ITG3200::getInterruptConfiguration(void)
{

    char tx = INT_CFG_REG;
    char rx;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);

    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);

    return rx;

}

void ITG3200::setInterruptConfiguration(char config)
{

    char tx[2];
    tx[0] = INT_CFG_REG;
    tx[1] = config;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);

}

bool ITG3200::isPllReady(void)
{

    char tx = INT_STATUS;
    char rx;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);

    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);

    //ITG_RDY bit is bit 4 of INT_STATUS register.
    if(rx & 0x04) {
        return true;
    } else {
        return false;
    }

}

bool ITG3200::isRawDataReady(void)
{

    char tx = INT_STATUS;
    char rx;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);

    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);

    //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
    if(rx & 0x01) {
        return true;
    } else {
        return false;
    }

}

int ITG3200::getWord(int regi)
{

    char tx = regi;
    char rx[2];

    i2c_.write(I2C_ADDRESS, &tx, 1);

    i2c_.read(I2C_ADDRESS, rx, 2);

    return swapExtend(rx);
}

float ITG3200::getTemperature()
{
    //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
    return 35.0 + ((getRawTemperature() + 13200)/280.0);

}

void ITG3200::getRawXYZ(int16_t readings[3])
{

    char tx = GYRO_XOUT_H_REG;
    char rx[2];

    i2c_.write(I2C_ADDRESS, &tx, 1);

    i2c_.read(I2C_ADDRESS, rx, 6);

    for(int i = 0; i < 3; i++)
        readings[i] = swapExtend(&rx[i * 2]);
}

void ITG3200::getXYZ(int16_t readings[3], Correction corr)
{
    getRawXYZ(readings);
    switch(corr) {
        case OffsetCorrection:
            for(int i = 0; i < 3; i++)
                readings[i] -= static_cast<int16_t>(offset[i]);
            break;
        case Calibration: {
            float temp = getTemperature();
            for(int i = 0; i < 3; i++)
                readings[i] -= slope[i] * temp + foffset[i];
        }
        break;
    }
}


char ITG3200::getPowerManagement(void)
{

    char tx = PWR_MGM_REG;
    char rx;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);

    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);

    return rx;

}

void ITG3200::setPowerManagement(char config)
{

    char tx[2];
    tx[0] = PWR_MGM_REG;
    tx[1] = config;

    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);

}

void ITG3200::calibrate(double time)
{
    long sum[3] = {0};
    int sumCount = 0;
    Timer t;
    t.start();
    float start_time;
    float period = 1.0 / 50.0; // 50Hz
    
    while(t.read() < time) {
        start_time = t.read();
        int16_t gyro[3];
        getRawXYZ(gyro);
        for(int i = 0; i < 3; i++)
            sum[i] += gyro[i];
        sumCount++;
        wait(period - (t.read()-start_time));
    }
    t.stop();

    // Avoid zero division
    if(0 < sumCount) {
        for(int i = 0; i < 3; i++)
            offset[i] = (double)sum[i] / (double)sumCount;
        // Update member variable only if successful
        calibSamples = sumCount;
    }
}