mpu6050.cpp

00001 #include "mpu6050.h"
00002 
00003 static const double RATE_ACC_LIST[4] = {
00004     16384.0,
00005     8192.0,
00006     4096.0,
00007     2048.0,
00008 };
00009 
00010 static const double RATE_GYRO_LIST[4] = {
00011     131.0,
00012     65.5,
00013     32.8,
00014     16.4,
00015 };
00016 
00017 MPU6050::MPU6050(PinName i2c_sda, PinName i2c_scl) : i2c(i2c_sda, i2c_scl), address(0x68 << 1), rate_accelemeter(RATE_ACC_LIST[0]), rate_gyroscope(RATE_GYRO_LIST[0])
00018 {
00019     setSleep(false);
00020     setMaxScale(MAX_ACC_2G, MAX_GYRO_250degpersec);
00021 }
00022 
00023 MPU6050::~MPU6050()
00024 {
00025 }
00026 
00027 char MPU6050::readByte(char reg)
00028 {
00029     int result_write = i2c.write(address, &reg, 1);
00030     if(result_write) {
00031         // failed
00032     }
00033     char read_data;
00034     int result_read = i2c.read(address, &read_data, 1);
00035     if(result_read) {
00036         // failed
00037     }
00038     return read_data;
00039 }
00040 
00041 void MPU6050::writeByte(char reg, char data)
00042 {
00043     char buf[2];
00044     buf[0] = reg;
00045     buf[1] = data;
00046     int result = i2c.write(address, buf, 2);
00047     if(result) {
00048         // failed
00049     }
00050 }
00051 
00052 void MPU6050::readAccelemeter(double &acc_x, double &acc_y, double &acc_z)
00053 {
00054     char h, l;
00055     short x, y, z;
00056     // read X
00057     h = readByte(0x3B);
00058     l = readByte(0x3C);
00059     x = h << 8 | l;
00060     // read Y
00061     h = readByte(0x3D);
00062     l = readByte(0x3E);
00063     y = h << 8 | l;
00064     // read Z
00065     h = readByte(0x3F);
00066     l = readByte(0x40);
00067     z = h << 8 | l;
00068     acc_x = (double)x / rate_accelemeter;
00069     acc_y = (double)y / rate_accelemeter;
00070     acc_z = (double)z / rate_accelemeter;
00071 }
00072 
00073 void MPU6050::readGyroscope(double &gyro_x, double &gyro_y, double &gyro_z)
00074 {
00075     char h, l;
00076     short x, y, z;
00077     // read X
00078     h = readByte(0x43);
00079     l = readByte(0x44);
00080     x = h << 8 | l;
00081     // read Y
00082     h = readByte(0x45);
00083     l = readByte(0x46);
00084     y = h << 8 | l;
00085     // read Z
00086     h = readByte(0x47);
00087     l = readByte(0x48);
00088     z = h << 8 | l;
00089     gyro_x = (double)x / rate_gyroscope;
00090     gyro_y = (double)y / rate_gyroscope;
00091     gyro_z = (double)z / rate_gyroscope;
00092 }
00093 
00094 void MPU6050::getMaxScale(int &acc, int &gyro)
00095 {
00096     char reg;
00097     reg = readByte(0x1C);
00098     acc = reg;
00099     reg = readByte(0x1B);
00100     gyro = reg;
00101 }
00102 
00103 void MPU6050::setMaxScale(int max_acc, int max_gyro)
00104 {
00105     switch(max_acc) {
00106     case MAX_ACC_2G:
00107         writeByte(0x1C, 0x00);
00108         break;
00109     case MAX_ACC_4G:
00110         writeByte(0x1C, 0x08);
00111         break;
00112     case MAX_ACC_8G:
00113         writeByte(0x1C, 0x10);
00114         break;
00115     case MAX_ACC_16G:
00116         writeByte(0x1C, 0x18);
00117         break;
00118     }
00119     rate_accelemeter = RATE_ACC_LIST[max_acc];
00120     switch(max_gyro) {
00121     case MAX_GYRO_250degpersec:
00122         writeByte(0x1B, 0x00);
00123         break;
00124     case MAX_GYRO_500degpersec:
00125         writeByte(0x1B, 0x08);
00126         break;
00127     case MAX_GYRO_1000degpersec:
00128         writeByte(0x1B, 0x10);
00129         break;
00130     case MAX_GYRO_2000degpersec:
00131         writeByte(0x1B, 0x18);
00132         break;
00133     }
00134     rate_gyroscope = RATE_GYRO_LIST[max_gyro];
00135 }
00136 
00137 void MPU6050::setSleep(bool sleep)
00138 {
00139     const char reg_sleep = 0x6B;
00140     const char current_state = readByte(reg_sleep);
00141     char write_data;
00142     if(sleep) {
00143         write_data = current_state | 0x40;
00144     } else {
00145         write_data = current_state & (~0x40);
00146     }
00147     writeByte(reg_sleep, write_data);
00148 }