MPU9250.cpp

00001 
00002 #include "mbed.h"
00003 #include "MPU9250.h"
00004 
00005 MPU9250::MPU9250(PinName sda,PinName scl) : i2c(sda, scl)
00006 {
00007 }
00008 
00009 MPU9250::~MPU9250()
00010 {
00011 }
00012 
00013 char MPU9250::read(char addr, char regist)
00014 {   
00015     char cmd[2];
00016     cmd[0]= regist;
00017     i2c.write(addr,cmd,1);
00018     i2c.read(addr,cmd,1);
00019     return cmd[0];
00020 }
00021 
00022 void MPU9250::write(char addr, char regist,char data)
00023 {
00024     char cmd[2];
00025     cmd[0]=regist;
00026     cmd[1]=data;
00027     i2c.write(addr,cmd,2);
00028 }
00029 
00030 char MPU9250::who()
00031 {   
00032     char _ID;
00033     _ID = read(ADDR_MPU, MPU_WHO);
00034     return _ID;
00035 }
00036 
00037 char MPU9250::AKwho()
00038 {
00039     char _ID;
00040     _ID = read(ADDR_AK, AK_WHO);
00041     return _ID;
00042 }
00043 
00044 void MPU9250::start()
00045 {
00046     //MPU9250 gyro&gyro start
00047     write(ADDR_MPU, MPU_PWR, 0x00);
00048     
00049     //mag start
00050     write(ADDR_MPU, MPU_MAG_INT, 0x02);
00051     wait(0.5);
00052     
00053 }
00054 
00055 void MPU9250::accelsetup(char mode)
00056 {
00057     char _mode;
00058     if(mode == 0){
00059         _mode = ACCEL_2G;
00060     }
00061     else if(mode == 1){
00062         _mode = ACCEL_4G;
00063     }
00064     else if(mode == 2){
00065         _mode = ACCEL_8G;
00066     }
00067     else if(mode == 3){
00068         _mode = ACCEL_16G;
00069     }
00070     
00071     write(ADDR_MPU, ACCEL_CONFIG,  _mode);
00072 }
00073 
00074 void MPU9250::gyrosetup(char mode)
00075 {
00076     char _mode;
00077     if(mode == 0){
00078         _mode = gyro_250DPS;
00079     }
00080     else if(mode == 1){
00081         _mode = gyro_500DPS;
00082     }
00083     else if(mode == 2){
00084         _mode = gyro_1000DPS;
00085     }
00086     else if(mode == 3){
00087         _mode = gyro_2000DPS;
00088     }
00089     
00090     write(ADDR_MPU, gyro_CONFIG, _mode);
00091 }
00092 
00093 void MPU9250::AKsetup(char mode)
00094 {
00095     char _mode;
00096     if(mode == 0){
00097         _mode = AK_8Hz;
00098     }
00099     else if(mode == 1){
00100         _mode = AK_100Hz;
00101     }
00102 
00103     write(ADDR_AK, AK_CNTL1, _mode);
00104 }
00105 
00106 void MPU9250::accel_read(char mode, float *A)
00107 {
00108     char id[2], data[6];
00109     int16_t ax, ay, az;
00110     char _mode;
00111     float accx, accy, accz;
00112     
00113     if(mode == 0){
00114         _mode = 2;
00115     }
00116     else if(mode == 1){
00117         _mode = 4;
00118     }
00119     else if(mode == 2){
00120         _mode = 8;
00121     }
00122     else if(mode == 3){
00123         _mode = 16;
00124     }
00125     
00126     id[0] = MPU_ACCELDATA;
00127     i2c.write(ADDR_MPU, id, 1);
00128     i2c.read(ADDR_MPU, data, 6);
00129     ax = (data[0] << 8) | data[1];
00130     ay = (data[2] << 8) | data[3];
00131     az = (data[4] << 8) | data[5];
00132     
00133     accx = ax * _mode / 32768.0 * 9.81;  //[m/s^2]に変換
00134     accy = ay * _mode / 32768.0 * 9.81;  //同様
00135     accz = az * _mode / 32768.0 * 9.81;  //同様
00136     
00137     *A = accx;
00138     *(A+1) = accy;
00139     *(A+2) = accz;
00140 
00141 }
00142 
00143 void MPU9250::gyro_read(char mode, float *G)
00144 {
00145     char id[2], data[6];
00146     int16_t gx, gy, gz;
00147     char _mode;
00148     float gyrox, gyroy, gyroz;
00149 
00150     
00151     if(mode == 0){
00152         _mode = 250;
00153     }
00154     else if(mode == 1){
00155         _mode = 500;
00156     }
00157     else if(mode == 2){
00158         _mode = 1000;
00159     }
00160     else if(mode == 3){
00161         _mode = 2000;
00162     }
00163     
00164     id[0] = MPU_GYRODATA;
00165     i2c.write(ADDR_MPU, id, 1);
00166     i2c.read(ADDR_MPU, data, 6);
00167     gx = (data[0] << 8) | data[1];
00168     gy = (data[2] << 8) | data[3];
00169     gz = (data[4] << 8) | data[5];
00170     
00171     gyrox = gx * _mode / 32768.0;       //[deg]
00172     gyroy = gy * _mode / 32768.0;
00173     gyroz = gz * _mode / 32768.0;
00174     
00175     *G = gyrox;
00176     *(G+1) = gyroy;
00177     *(G+2) = gyroz;
00178 
00179 }
00180 
00181 void MPU9250::mag_read(float *M)
00182 {
00183     char id[2], data[6];
00184     uint8_t ST1Bit;
00185     int16_t mx, my, mz;
00186     float magx, magy, magz;
00187 
00188     ST1Bit = read(ADDR_AK, ST1);    //mag read bit
00189         
00190     if((ST1Bit & 0x01))
00191     {
00192         id[0] = AK_DATA;
00193         i2c.write(ADDR_AK, id, 1);
00194         i2c.read(ADDR_AK, data, 7);
00195      }
00196         
00197     my = (data[1] << 8) | data[0];
00198     mx = (data[3] << 8) | data[2];
00199     mz = -((data[5] << 8) | data[4]);
00200         
00201     magx = mx * 4800.0f / 32768; //[ut]に変換している
00202     magy = my * 4800.0f / 32768; //同様
00203     magz = mz * 4800.0f / 32768; //同様
00204         
00205     *M = magx;
00206     *(M+1) = magy;
00207     *(M+2) = magz;
00208         
00209 }