File content as of revision 8:b1570b99df9e:

/**
 * Includes
 */
#include "MPU6050.h"

MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
    this->setSleepMode(false);
    
    //Initializations:
    currentGyroRange = 0;
    currentAcceleroRange=0;
}

//--------------------------------------------------
//-------------------General------------------------
//--------------------------------------------------

void MPU6050::write(char address, char data) {
    char temp[2];
    temp[0]=address;
    temp[1]=data;
    
    connection.write(MPU6050_ADDRESS * 2,temp,2);
}

char MPU6050::read(char address) {
    char retval;
    connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
    connection.read(MPU6050_ADDRESS * 2, &retval, 1);
    return retval;
}

void MPU6050::read(char address, char *data, int length) {
    connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
    connection.read(MPU6050_ADDRESS * 2, data, length);
}

void MPU6050::setSleepMode(bool state) {
    char temp;
    temp = this->read(MPU6050_PWR_MGMT_1_REG);
    if (state == true)
        temp |= 1<<MPU6050_SLP_BIT;
    if (state == false)
        temp &= ~(1<<MPU6050_SLP_BIT);
    this->write(MPU6050_PWR_MGMT_1_REG, temp);
}

bool MPU6050::testConnection( void ) {
    char temp;
    temp = this->read(MPU6050_WHO_AM_I_REG);
    return (temp == (MPU6050_ADDRESS & 0xFE));
}

void MPU6050::setBW(char BW) {
    char temp;
    BW=BW & 0x07;
    temp = this->read(MPU6050_CONFIG_REG);
    temp &= 0xF8;
    temp = temp + BW;
    this->write(MPU6050_CONFIG_REG, temp);
}

void MPU6050::setI2CBypass(bool state) {
    char temp;
    temp = this->read(MPU6050_INT_PIN_CFG);
    if (state == true)
        temp |= 1<<MPU6050_BYPASS_BIT;
    if (state == false)
        temp &= ~(1<<MPU6050_BYPASS_BIT);
    this->write(MPU6050_INT_PIN_CFG, temp);
}

//--------------------------------------------------
//----------------Accelerometer---------------------
//--------------------------------------------------

void MPU6050::setAcceleroRange( char range ) {
    char temp;
    range = range & 0x03;
    currentAcceleroRange = range;
    
    temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
    temp &= ~(3<<3);
    temp = temp + (range<<3);
    this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
}

int MPU6050::getAcceleroRawX( void ) {
    short retval;
    char data[2];
    this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
    retval = (data[0]<<8) + data[1];
    return (int)retval;
}
    
int MPU6050::getAcceleroRawY( void ) {
    short retval;
    char data[2];
    this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
    retval = (data[0]<<8) + data[1];
    return (int)retval;
}

int MPU6050::getAcceleroRawZ( void ) {
    short retval;
    char data[2];
    this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
    retval = (data[0]<<8) + data[1];
    return (int)retval;
}

void MPU6050::getAcceleroRaw( int *data ) {
    char temp[6];
    this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
    data[0] = (int)(short)((temp[0]<<8) + temp[1]);
    data[1] = (int)(short)((temp[2]<<8) + temp[3]);
    data[2] = (int)(short)((temp[4]<<8) + temp[5]);
}

void MPU6050::getAccelero( float *data ) {
    int temp[3];
    this->getAcceleroRaw(temp);
    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
        data[0]=(float)temp[0] / 16384.0 * 9.81;
        data[1]=(float)temp[1] / 16384.0 * 9.81;
        data[2]=(float)temp[2] / 16384.0 * 9.81;
        }
    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
        data[0]=(float)temp[0] / 8192.0 * 9.81;
        data[1]=(float)temp[1] / 8192.0 * 9.81;
        data[2]=(float)temp[2] / 8192.0 * 9.81;
        }
    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
        data[0]=(float)temp[0] / 4096.0 * 9.81;
        data[1]=(float)temp[1] / 4096.0 * 9.81;
        data[2]=(float)temp[2] / 4096.0 * 9.81;
        }
    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
        data[0]=(float)temp[0] / 2048.0 * 9.81;
        data[1]=(float)temp[1] / 2048.0 * 9.81;
        data[2]=(float)temp[2] / 2048.0 * 9.81;
        }
    
    #ifdef DOUBLE_ACCELERO
        data[0]*=2;
        data[1]*=2;   
        data[2]*=2;
    #endif   
}

//function for getting angles from accelerometer
void MPU6050::getAcceleroAngle( float *data ) {
    float temp[3];
    this->getAccelero(temp);
    
    data[X_AXIS] = -1*atan (-1*temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
    data[Y_AXIS] = -1*atan (temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
    data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //not sure what is this one :D
    
}
void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
    float gyro[3];      
    float accAngle[3];
    
    for (int i = 0; i < 3; i++) //initialise to 0.0 offsets
    {
        accOffset[i] = 0.0;
        gyroOffset[i] = 0.0;
    }
    
    for (int i = 0; i < sampleSize; i++)
    {
        this->getGyro(gyro); //take real life measurements  
        this->getAcceleroAngle (accAngle);
        
        for (int j = 0; j < 3; j++)
        {
            *(accOffset+j) += accAngle[j]/sampleSize;    //average measurements
            *(gyroOffset+j) += gyro[j]/sampleSize;
        }
        wait (0.01);    //wait between each reading for accuracy (maybe will not need later on) ****))#$(#@)$@#)(#@(%)@#(%@)(#%#@$--------------
    } 
}

void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime)
{
    float gyro[3];
    float accAngle[3];
    
    this->getGyro(gyro);
    this->getAcceleroAngle(accAngle);
    
    for (int i = 0; i < 3; i++)
    {
        gyro[i] -= gyroOffset[i];
        accAngle[i] -= accOffset[i];
    }
    
    angle[X_AXIS] = ALPHA * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[X_AXIS];  //apply filter on both reading to get all angles
    angle[Y_AXIS] = ALPHA * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[Y_AXIS];
  //angle[Z_AXIS] = ALPHA * (angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[Z_AXIS];
    angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime);  //this is Yaw hopefully :D
    //Y = atan2(rawY,rawZ) * 180 / PI;  //This spits out values between -180 to 180 (360 degrees)    
}

void MPU6050::enableInt( void )
{
    char temp;
    temp = this->read(MPU6050_RA_INT_ENABLE);
    temp |= 0x01;
    this->write(MPU6050_RA_INT_ENABLE, temp);
}
void MPU6050::disableInt ( void )
{
    char temp;
    temp = this->read(MPU6050_RA_INT_ENABLE);
    temp &= 0xFE;
    this->write(MPU6050_RA_INT_ENABLE, temp);
}
//--------------------------------------------------
//------------------Gyroscope-----------------------
//--------------------------------------------------
void MPU6050::setGyroRange( char range ) {
    char temp;
    currentGyroRange = range;
    range = range & 0x03;
    temp = this->read(MPU6050_GYRO_CONFIG_REG);
    temp &= ~(3<<3);
    temp = temp + range<<3;
    this->write(MPU6050_GYRO_CONFIG_REG, temp);
}

int MPU6050::getGyroRawX( void ) {
    short retval;
    char data[2];
    this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
    retval = (data[0]<<8) + data[1];
    return (int)retval;
}
    
int MPU6050::getGyroRawY( void ) {
    short retval;
    char data[2];
    this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
    retval = (data[0]<<8) + data[1];
    return (int)retval;
}

int MPU6050::getGyroRawZ( void ) {
    short retval;
    char data[2];
    this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
    retval = (data[0]<<8) + data[1];
    return (int)retval;
}

void MPU6050::getGyroRaw( int *data ) {
    char temp[6];
    this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
    data[0] = (int)(short)((temp[0]<<8) + temp[1]);
    data[1] = (int)(short)((temp[2]<<8) + temp[3]);
    data[2] = (int)(short)((temp[4]<<8) + temp[5]);
}

void MPU6050::getGyro( float *data ) {
    int temp[3];
    this->getGyroRaw(temp);
    if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
        data[0]=(float)temp[0] / 301.0;
        data[1]=(float)temp[1] / 301.0;
        data[2]=(float)temp[2] / 301.0;
        }               //7505.5
    if (currentGyroRange == MPU6050_GYRO_RANGE_500){
        data[0]=(float)temp[0] / 3752.9;
        data[1]=(float)temp[1] / 3752.9;
        data[2]=(float)temp[2] / 3752.9;
        }
    if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
        data[0]=(float)temp[0] / 1879.3;;
        data[1]=(float)temp[1] / 1879.3;
        data[2]=(float)temp[2] / 1879.3;
        }
    if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
        data[0]=(float)temp[0] / 939.7;
        data[1]=(float)temp[1] / 939.7;
        data[2]=(float)temp[2] / 939.7;
        }
}
//--------------------------------------------------
//-------------------Temperature--------------------
//--------------------------------------------------
int MPU6050::getTempRaw( void ) {
    short retval;
    char data[2];
    this->read(MPU6050_TEMP_H_REG, data, 2);
    retval = (data[0]<<8) + data[1];
    return (int)retval;
}

float MPU6050::getTemp( void ) {
    float retval;
    retval=(float)this->getTempRaw();
    retval=(retval+521.0)/340.0+35.0;
    return retval;
}