File content as of revision 6:b4f5edf825be:

#include "mbed.h"
#include "MPU6050.h"

#define ADDRESS 0xD1 //68 shifted left: 7-bit to 8-bit address conversion

MPU6050::MPU6050(PinName sda, PinName scl):
    _MPU6050(sda, scl)
{
    _MPU6050.frequency(400000);
}

void MPU6050::start(void)
{
    write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00);
    write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x00);//gyro +-500deg/s //0x10);//gyro +-1000deg/s
    write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x00);//accel +-2G //0x08);//accel +-4G
    write_reg(ADDRESS,MPU6050_CONFIG,0x06);//2 - bandwidth 94Hz, delay 3 mS; 3 - bandwidth 44Hz, delay 5 mS; 4 - bandwidth 21Hz, delay 8.5 mS; 5 - bandwidth 10Hz, delay 14 mS
    write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x0);//sample rate 1000Hz (1000/5)
}

char MPU6050::getID(void)
{
    char devID;
    read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID);
    return devID;
}

bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az)
{
    char data[6];
    char data2[6];
    if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
        read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
        *gx = float(short(data[0] << 8 | data[1]))*2.663e-4/2; //rad/s
        *gy =  float(short(data[2] << 8 | data[3]))*2.663e-4/2;
        *gz =  float(short(data[4] << 8 | data[5]))*2.663e-4/2;
        *ax = float(short(data2[0] << 8 | data2[1]))*5.9875e-6; //m/s^2
        *ay =  float(short(data2[2] << 8 | data2[3]))*5.9875e-6;
        *az =  float(short(data2[4] << 8 | data2[5]))*5.9875e-6;
        return true;
    }
    return false;
}

bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *az)
{
    char data[6];
    char data2[6];
    if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
        read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
        *gx = int(short(data[0] << 8 | data[1]));
        *gy =  int(short(data[2] << 8 | data[3]));
        *gz =  int(short(data[4] << 8 | data[5]));
        *ax = int(short(data2[0] << 8 | data2[1]));
        *ay =  int(short(data2[2] << 8 | data2[3]));
        *az =  int(short(data2[4] << 8 | data2[5]));
        return true;
    }
    return false;
}

bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v)
{
    char data[2] = {addr_reg, v};
    return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0;
}

bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v)
{
    char data = addr_reg;
    bool result = false;
//    __disable_irq();
    if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) {
        *v = data;
        result = true;
    }
//    __enable_irq();
    return result;
}


bool MPU6050::read_data(char sad, char sub, char *buf, int length)
{
    if (length > 1) sub |= 0x80;

    return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0;
}

/* sample code for Nucleo-F042K6
#include "mbed.h"
#include "MPU6050.h"

MPU6050 mpu(D7,D8);
Serial pc(USBTX,USBRX);

float gx,gy,gz,ax,ay,az;

int main()
{
    pc.baud(115200);
    if(mpu.getID()==0x68) {
        pc.printf("MPU6050 OK");
        wait(1);
    } else {
        pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
        while(1) {
        }
    }
    mpu.start();
    while(1) {
        mpu.read(&gx,&gy,&gz,&ax,&ay,&az);
        pc.printf("gx,gy,gz,ax,ay,az %.1f,%.1f,%.1f,%.2f,%.2f,%.2f\r\n",gx,gy,gz,ax,ay,az);
        wait(0.1);
    }
}
*/