mpu9250_i2c.cpp

00001 #include "mbed.h"
00002 #include "mpu9250_i2c.h"
00003 
00004 char mpu9250::_addr = SLAVE_ADDR_HIGH;
00005 double mpu9250::acc_coef = ACC_LSB;
00006 double mpu9250::gyro_coef = GYRO_LSB;
00007 double mpu9250::mag_coef = MAG_LSB;
00008 double mpu9250::acc_offset[3] = {0,0,0};
00009 double mpu9250::gyro_offset[3] = {0,0,0};
00010 double mpu9250::mag_offset[3] = {0,0,0};
00011 
00012 mpu9250::mpu9250(I2C &_i2c, AD0 celect){
00013     
00014     _nine = &_i2c;
00015     if(celect == AD0_HIGH) _addr = SLAVE_ADDR_HIGH;   
00016     else _addr = SLAVE_ADDR_LOW;   
00017     
00018     _nine->frequency(400000);//400kHz
00019     init();
00020 }
00021 void mpu9250::frequency(int Hz){
00022     _nine->frequency(Hz);   
00023 }
00024 bool mpu9250::senserTest (){
00025     if(readReg(_addr, WHO_AM_I_MPU9250) == 0x71){
00026         return true;   
00027     }   
00028     else return false;
00029 }
00030 
00031 bool mpu9250::mag_senserTest(){
00032      if(readReg(MAG_ADDR, WIA) == 0x48){
00033         return true;   
00034     }   
00035     else return false;
00036 }
00037 
00038 void mpu9250::setOffset(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz){
00039     gyro_offset[0] = gx;
00040     gyro_offset[1] = gy;
00041     gyro_offset[2] = gz;
00042     acc_offset[0] = ax;
00043     acc_offset[1] = ay;
00044     acc_offset[2] = az;
00045     mag_offset[0] = mx;
00046     mag_offset[1] = my;
00047     mag_offset[2] = mz;
00048 }
00049 
00050 void mpu9250::setGyro(GYRO_RANGE g_range){
00051     char fchoice = readReg(_addr, GYRO_CONFIG) & 0x03;
00052     char send;
00053     if(g_range == _250DPS){
00054         send = 0x00 | fchoice;
00055         gyro_coef = GYRO_LSB;
00056     } 
00057     else if(g_range == _500DPS){
00058         send = 0x08 | fchoice;
00059         gyro_coef = GYRO_LSB * 2.0; 
00060     }
00061     else if(g_range == _1000DPS){
00062         send = 0x10 | fchoice;   
00063         gyro_coef = GYRO_LSB * 4.0;
00064     }
00065     else if(g_range == _2000DPS){
00066         send = 0x18 | fchoice;
00067         gyro_coef = GYRO_LSB * 8.0;   
00068     }
00069     writeReg(_addr, GYRO_CONFIG, send);
00070 }
00071 void mpu9250::setAcc(ACC_RANGE a_range){
00072     char send;
00073     if(a_range == _2G){
00074         send = 0x00;
00075         acc_coef = ACC_LSB;
00076     }
00077     else if(a_range == _4G){
00078         send = 0x08;
00079         acc_coef = ACC_LSB * 2.0;   
00080     }
00081     else if(a_range == _8G){
00082         send = 0x10;
00083         acc_coef = ACC_LSB * 4.0;   
00084     }
00085     else if(a_range == _16G){
00086         send = 0x18;
00087         acc_coef = ACC_LSB * 8.0;   
00088     }
00089     writeReg(_addr, ACCEL_CONFIG, send);
00090 }
00091 
00092 void mpu9250::init(){
00093     
00094     acc_coef = ACC_LSB;
00095     gyro_coef = GYRO_LSB;
00096     mag_coef = MAG_LSB;
00097     
00098     writeReg(MAG_ADDR, 0x6B, 0x00);
00099     wait_us(70);
00100     writeReg(_addr, 0x37, 0x02);
00101     wait_us(70);
00102     writeReg(MAG_ADDR, CNTL1, 0x16); 
00103     wait_us(70);
00104     setAcc(_4G);
00105     wait_us(70);
00106     setGyro(_500DPS);      
00107 }
00108 
00109 template<typename T>void mpu9250::getAcc(T *acc){
00110     char data[6];  
00111     short sign;
00112     readReg(_addr, ACCEL_XOUT_H, data, 6);
00113     sign = ((short)data[0] << 8) | (short)data[1];
00114     acc[0] = (double)sign * acc_coef - acc_offset[0];
00115     sign = ((short)data[2] << 8) | (short)data[3];
00116     acc[1] = (double)sign * acc_coef - acc_offset[1];
00117     sign = ((short)data[4] << 8) | (short)data[5];
00118     acc[2] = (double)sign * acc_coef - acc_offset[2];
00119 }
00120 template<typename T>void mpu9250::getAcc(T *ax, T *ay, T *az){
00121     double acc[3];
00122     getAcc(acc);
00123     *ax = acc[0];
00124     *ay = acc[1];
00125     *az = acc[2];   
00126 }
00127 template<typename T>void mpu9250::getGyro(T *gyro){
00128     char data[6];  
00129     short sign;
00130     readReg(_addr, GYRO_XOUT_H, data, 6);
00131     sign = ((short)data[0] << 8) | (short)data[1];
00132     gyro[0] = (double)sign * gyro_coef - gyro_offset[0];
00133     sign = ((short)data[2] << 8) | (short)data[3];
00134     gyro[1] = (double)sign * gyro_coef - gyro_offset[1];
00135     sign = ((short)data[4] << 8) | (short)data[5];
00136     gyro[2] = (double)sign * gyro_coef - gyro_offset[2];
00137 }
00138 template<typename T>void mpu9250::getGyro(T *gx, T *gy, T *gz){
00139     double gyro[3];
00140     getGyro(gyro);
00141     *gx = gyro[0];
00142     *gy = gyro[1];
00143     *gz = gyro[2];   
00144 }
00145 template<typename T>void mpu9250::getMag(T *mag){
00146     char data[8];   
00147     short sign;
00148     readReg(MAG_ADDR, ST1, data, 8);
00149     sign = ((short)data[2] << 8) | (short)data[1];
00150     mag[1] = (double)sign * mag_coef - mag_offset[1];
00151     sign = ((short)data[4] << 8) | (short)data[3];
00152     mag[0] = (double)sign * mag_coef - mag_offset[0];
00153     sign = ((short)data[6] << 8) | (short)data[5];
00154     mag[2] = (double)sign * -mag_coef - mag_offset[2];
00155 }
00156 template<typename T>void mpu9250::getMag(T *mx, T *my, T *mz){
00157     double mag[3];
00158     getMag(mag);
00159     *mx = mag[0];
00160     *my = mag[1];
00161     *mz = mag[2];   
00162 }
00163 template<typename T>void mpu9250::getGyroAcc(T *imu){
00164     char data[14];  
00165     short sign;
00166     readReg(_addr, ACCEL_XOUT_H, data, 14);
00167     sign = ((short)data[0] << 8) | (short)data[1];
00168     imu[3] = (double)sign * acc_coef - acc_offset[0];
00169     sign = ((short)data[2] << 8) | (short)data[3];
00170     imu[4] = (double)sign * acc_coef - acc_offset[1];
00171     sign = ((short)data[4] << 8) | (short)data[5];
00172     imu[5] = (double)sign * acc_coef - acc_offset[2]; 
00173     
00174     sign = ((short)data[8] << 8) | (short)data[9];
00175     imu[0] = (double)sign * gyro_coef - gyro_offset[0];
00176     sign = ((short)data[10] << 8) | (short)data[11];
00177     imu[1] = (double)sign * gyro_coef - gyro_offset[1];
00178     sign = ((short)data[12] << 8) | (short)data[13];
00179     imu[2] = (double)sign * gyro_coef - gyro_offset[2];   
00180 }
00181 
00182 void mpu9250::setAccLPF(A_BAND_WIDTH band){
00183     writeReg(_addr, ACCEL_CONFIG2, band);
00184     wait_us(70);  
00185 }
00186 
00187 template void mpu9250::getAcc<double>(double *ax, double *ay, double *az);
00188 template void mpu9250::getAcc<float>(float *ax, float *ay, float *az);
00189 template void mpu9250::getAcc<double>(double *acc);
00190 template void mpu9250::getAcc<float>(float *acc);    
00191     
00192 template void mpu9250::getGyro<double>(double *gx, double *gy, double *gz);
00193 template void mpu9250::getGyro<float>(float *gx, float *gy, float *gz);
00194 template void mpu9250::getGyro<double>(double *gyro);
00195 template void mpu9250::getGyro<float>(float *gyro);
00196     
00197 template void mpu9250::getMag<double>(double *mx, double *my, double *mz);
00198 template void mpu9250::getMag<float>(float *mx, float *my, float *mz);
00199 template void mpu9250::getMag<double>(double *mag);    
00200 template void mpu9250::getMag<float>(float *mag);  
00201 
00202 template void mpu9250::getGyroAcc<double>(double *imu);//gx,gy,gz,ax,ay,az
00203 template void mpu9250::getGyroAcc<float>(float *imu);//gx,gy,gz,ax,ay,az
00204