File content as of revision 1:64f1aefe1842:

#include "ENGO333_MPU9150.h"

const float GRAVITY = 9.8086;

ENGO333_MPU9150::ENGO333_MPU9150() : i2c(p28, p27)
{
    i2c.setSpeed(MPU9150_I2C_FAST_MODE);
    setSleepMode(false);
    setAccelRange(MPU9150_ACCEL_RANGE_2G);
    
    measAccel[0] = measAccel[1] = measAccel[2] = 0;
}

void ENGO333_MPU9150::setSleepMode(bool state)
{
    char temp;
    temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_PWR_MGMT_1_REG);
    if (state == true)
        temp |= 1 << MPU9150_SLEEP_BIT;
    if (state == false)
        temp &= ~(1 << MPU9150_SLEEP_BIT);
    i2c.writeOneByte(MPU9150_ADDRESS, MPU9150_PWR_MGMT_1_REG, temp);
}

bool ENGO333_MPU9150::TestConnection()
{
    char temp;
    temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_WHO_AM_I_REG);
    return (temp == (0x68 & 0xFE));
}

void ENGO333_MPU9150::setAccelRange(char range)
{
    char temp;
    range = range & 0x03;
    
    temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_ACCEL_CONFIG_REG);
    temp &= ~(3 << 3);
    temp = temp + (range << 3);
    i2c.writeOneByte(MPU9150_ADDRESS, MPU9150_ACCEL_CONFIG_REG, temp);
}

void ENGO333_MPU9150::ReadAccelerometers()
{
    char temp[6];
    
    while ((i2c.readOneByte(MPU9150_ADDRESS, MPU9150_INT_STATUS_REG) & 0x01) != 0x01)
    {
        // wait data to be ready
    }
    
    i2c.readBytes(MPU9150_ADDRESS, MPU9150_ACCEL_XOUT_H_REG, temp, 6);
    measAccel[0] = ((short)((temp[0] << 8) + temp[1])) / 16384.0 * GRAVITY;
    measAccel[1] = ((short)((temp[2] << 8) + temp[3])) / 16384.0 * GRAVITY;
    measAccel[2] = ((short)((temp[4] << 8) + temp[5])) / 16384.0 * GRAVITY;
}

float ENGO333_MPU9150::GetAccelX() const
{
    return measAccel[0];
}

float ENGO333_MPU9150::GetAccelY() const
{
    return measAccel[1];
}

float ENGO333_MPU9150::GetAccelZ() const
{
    return measAccel[2];
}