File content as of revision 0:70eb7a2966f1:

#include "MPU6050.h"
#define pi 3.14159265358979323846

 

void MPU6050::writeByte(char pointer, char byte)
{
    char write[2];
    write[0]=pointer;           // I2C sends MSB first. Namely  >>|subAddress|>>|data|
    write[1]=byte;
    i2c.write(slaveAddress, write, 2, 0);  // i2c.write(int address, char* data, int length, bool repeated=false);  
}


char MPU6050::readByte(char pointer)
{
    char read;  
    i2c.write(slaveAddress,&pointer, 1, 1);//write the register that needs to be read
    i2c.read(slaveAddress,&read, 1, 0);
    return read; 

}

void MPU6050::readBytes(char pointer, char *bytes, char length)
{
      
    i2c.write(slaveAddress,&pointer,1,1);
    i2c.read(slaveAddress,bytes,length);
}

void MPU6050::whoAmI()
{
    uint8_t whoAmI = this->readByte(0x75);
    pc.printf("0x%x ", whoAmI);//debug
    if(whoAmI == 0x68)//if a0 pin is floating or grounded should return 0x68
    {
        pc.printf("connection detected\n\r");
    }
    
    else//if no connection detected
    {
        pc.printf("check connection");
    }
}

void MPU6050::sleep(bool state) {
    char temp;
    temp = this->readByte(0x6B);//MPU6050_PWR_MGMT_1_REG
    if (state == true)
        temp |= 1<<6;
    if (state == false)
        temp &= 0<<6;
    this->writeByte(0x6B, temp);
}

void MPU6050::init()
{
    i2c.frequency(400000); //400 kHz
    sleep(false);
    wait_ms(100);
    uint8_t temp = this->readByte(0x1A);
    this->writeByte(0x1A, temp & 0xC0);//DSYNC disabled bandwidth selected 44HZ for accelometer and 42HZ for gyroscope
    temp = readByte(0x19);
    this->writeByte(0x19, temp | 0x04);//sample rate = 200hz
    
     
    temp = readByte(0x1B);
    this->writeByte(0x1B, temp & ~0xE0);
    temp = readByte(0x1B);      // clear self test x,y,z for gyro
    this->writeByte(0x1B, temp & ~0x18); 
    temp = readByte(0x1B);     // clear FS_SEL
    this->writeByte(0x1B, temp | 0<<3 | 0<<4);  // Set range 2000 degree/sec    
    temp = readByte(0x1C);
    this->writeByte(0x1C, temp & ~0xE0); 
    temp = readByte(0x1C);     
    this->writeByte(0x1C, temp & ~0x18);  
    temp = readByte(0x1C);    
    this->writeByte(0x1C, temp | 0<<3 | 0<<4); //16G
    
}

void MPU6050::gyro(float *val)
{
    char temp_val[6]; 
    this->readBytes(0x43, temp_val, 6);
    int16_t tempy_val[3];
    tempy_val[0] = (int16_t)((temp_val[0]<<8) | temp_val[1]);//patch the gyro value
    tempy_val[1] = (int16_t)((temp_val[2]<<8) | temp_val[3]);
    tempy_val[2] = (int16_t)((temp_val[4]<<8) | temp_val[5]);
    val[0] = (float)tempy_val[0]/131;//divide by sensitivity
    val[1] = (float)tempy_val[1]/131;
    val[2] = (float)tempy_val[2]/131;
}

void MPU6050::acc(float *val)
{
    char temp_val[6]; 
    this->readBytes(0x3B, temp_val, 6);
    int16_t tempy_val[3];
    tempy_val[0] = (int16_t)((temp_val[0]<<8) | temp_val[1]);
    tempy_val[1] = (int16_t)((temp_val[2]<<8) | temp_val[3]);
    tempy_val[2] = (int16_t)((temp_val[4]<<8) | temp_val[5]);
    
    val[0] = (float)tempy_val[0]/16384;
    val[1] = (float)tempy_val[1]/16384;
    val[2] = (float)tempy_val[2]/16384;//divide by sensitivity
}

void MPU6050::filtered(float old_p, float *val, float time, float offset, float sp)//complimentary filter
{
    float gyro_val[3], acc_val[3];
    float pitch_temp;
    gyro(gyro_val);
    acc(acc_val);               
    val[0] = old_p + ((gyro_val[0]-offset)*time);//gyro uses the previous added error to get position + the current value*time
    float gravity_sum = abs(acc_val[0]) + abs(acc_val[1]) + abs(acc_val[2]);
    if (0.5 < gravity_sum && gravity_sum < 2)//if accelerometer value is acceptable
    {
        float r = sqrt(acc_val[0]*acc_val[0]+acc_val[1]*acc_val[1]+acc_val[2]*acc_val[2]);
        float x = acos(acc_val[0]/r);//get angle of x axis
        x = x*180/pi;//convert to degree
        val[0] = val[0] * 0.02 + (x-sp) * 0.98;//complimentary filter
    }
}