MPU6050.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 
00004 #define ADDRESS 0xD1
00005 
00006 MPU6050::MPU6050(I2C *iic):
00007     _MPU6050(iic)
00008 {
00009 }
00010 
00011 void MPU6050::start(void)
00012 {
00013     write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00);//disable sleep mode
00014     write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x10);//gyro +-1000deg/s
00015     write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x08);//accel +-4G
00016     write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 470Hz
00017     //to set more, see MPU6000 Register Map
00018 }
00019 
00020 char MPU6050::getID(void)
00021 {
00022     char devID;
00023     read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID);
00024     return devID;
00025 }
00026 
00027 bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az)
00028 {
00029     char data[6];
00030     char data2[6];
00031     if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
00032         read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
00033         *gx = float(short(data[0] << 8 | data[1]))*0.0305f;
00034         *gy =  float(short(data[2] << 8 | data[3]))*0.0305f;
00035         *gz =  float(short(data[4] << 8 | data[5]))*0.0305f;
00036         *ax = float(short(data2[0] << 8 | data2[1]))*0.000122f;
00037         *ay =  float(short(data2[2] << 8 | data2[3]))*0.000122f;
00038         *az =  float(short(data2[4] << 8 | data2[5]))*0.000122f;
00039         return true;
00040     }
00041     return false;
00042 }
00043 
00044 bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *az)
00045 {
00046     char data[6];
00047     char data2[6];
00048     if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
00049         read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
00050         *gx = int(short(data[0] << 8 | data[1]));
00051         *gy =  int(short(data[2] << 8 | data[3]));
00052         *gz =  int(short(data[4] << 8 | data[5]));
00053         *ax = int(short(data2[0] << 8 | data2[1]));
00054         *ay =  int(short(data2[2] << 8 | data2[3]));
00055         *az =  int(short(data2[4] << 8 | data2[5]));
00056         return true;
00057     }
00058     return false;
00059 }
00060 
00061 bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v)
00062 {
00063     char data[2] = {addr_reg, v};
00064     return MPU6050::_MPU6050->write(addr_i2c, data, 2) == 0;
00065 }
00066 
00067 bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v)
00068 {
00069     char data = addr_reg;
00070     bool result = false;
00071     __disable_irq();
00072     if ((_MPU6050->write(addr_i2c, &data, 1) == 0) && (_MPU6050->read(addr_i2c, &data, 1) == 0)) {
00073         *v = data;
00074         result = true;
00075     }
00076     __enable_irq();
00077     return result;
00078 }
00079 
00080 
00081 bool MPU6050::read_data(char sad, char sub, char *buf, int length)
00082 {
00083     if (length > 1) sub |= 0x80;
00084 
00085     return _MPU6050->write(sad, &sub, 1, true) == 0 && _MPU6050->read(sad, buf, length) == 0;
00086 }
00087 
00088 /* sample code for Nucleo-F042K6
00089 #include "mbed.h"
00090 #include "MPU6050.h"
00091 
00092 MPU6050 mpu(D7,D8);
00093 Serial pc(USBTX,USBRX);
00094 
00095 float gx,gy,gz,ax,ay,az;
00096 
00097 int main()
00098 {
00099     pc.baud(115200);
00100     if(mpu.getID()==0x68) {
00101         pc.printf("MPU6050 OK");
00102         wait(1);
00103     } else {
00104         pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
00105         while(1) {
00106         }
00107     }
00108     mpu.start();
00109     while(1) {
00110         mpu.read(&gx,&gy,&gz,&ax,&ay,&az);
00111         pc.printf("gx,gy,gz,ax,ay,az %.1f,%.1f,%.1f,%.2f,%.2f,%.2f\r\n",gx,gy,gz,ax,ay,az);
00112         wait(0.1);
00113     }
00114 }
00115 */