ICM20948.cpp

00001 #include "mbed.h"
00002 #include "ICM20948.h"
00003  
00004 ICM20948::ICM20948(PinName sda, PinName sck) : _i2c(sda, sck)
00005 {
00006     _i2c.frequency(400000);
00007     gyroBias[0] = 0;
00008     gyroBias[1] = 0;
00009     gyroBias[2] = 0;
00010 }
00011  
00012 void ICM20948::ICM_WriteByte(char ICM20948_reg, char ICM20948_data)
00013 {
00014     char data_out[2];
00015     data_out[0]=ICM20948_reg;
00016     data_out[1]=ICM20948_data;
00017     _i2c.write(ICM20948_slave_addr, data_out, 2, 0);
00018 }
00019  
00020 char ICM20948::ICM_ReadByte(char ICM20948_reg)
00021 {
00022     char data_out[1], data_in[1];
00023     data_out[0] = ICM20948_reg;
00024     _i2c.write(ICM20948_slave_addr, data_out, 1, 1);
00025     _i2c.read(ICM20948_slave_addr, data_in, 1, 0);
00026     return (data_in[0]);
00027 }
00028  
00029 // Communication test: WHO_AM_I register reading
00030 int ICM20948::whoAmI()
00031 {
00032     char whoAmI = ICM_ReadByte(ICM20948_WHO_AM_I);   // Should return 0x68
00033 
00034     if(whoAmI==0xea) {
00035         return 68;
00036     } else {
00037         return -1;
00038     }
00039  
00040 }
00041  
00042 void ICM20948::powerOn()
00043 {
00044     // USER_BANK_0
00045     ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_0);
00046     ICM_WriteByte(ICM20948_PWR_MGMT_1, 0x01);
00047     //ICM_WriteOneByte(ICM20948_PWR_MGMT_2, (0x38 | 0x07));
00048     ICM_WriteByte(ICM20948_PWR_MGMT_2, (0x38 | 0x07));
00049     ThisThread::sleep_for(10ms);
00050     //ICM_WriteOneByte(ICM20948_PWR_MGMT_2, (0x00 | 0x00));
00051     ICM_WriteByte(ICM20948_PWR_MGMT_2, (0x00 | 0x00));
00052 }
00053  
00054 void ICM20948::init(char accel_conf, char accel_div, char gyro_conf, char gyro_div)
00055 {
00056     powerOn();
00057 
00058     switch((accel_conf >> 1) & 0x03)
00059     {
00060         case 0:
00061             aRes = 2.0/32768.0;
00062             break;
00063         case 1:
00064             aRes = 4.0/32768.0;
00065             break;
00066         case 2:
00067             aRes = 8.0/32768.0;
00068             break;
00069         case 3:
00070             aRes = 16.0/32768.0;
00071             break;
00072     }
00073     switch((gyro_conf >> 1) & 0x03)
00074     {
00075         case 0:
00076             gRes = 250.0/32768.0;
00077             break;
00078         case 1:
00079             gRes = 500.0/32768.0;
00080             break;
00081         case 2:
00082             gRes = 1000.0/32768.0;
00083             break;
00084         case 3:
00085             gRes = 2000.0/32768.0;
00086             break;
00087     }
00088     // USER_BANK_0 to access data
00089     ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_2);
00090     ICM_WriteByte(ICM20948_GYRO_CONFIG_1, gyro_conf);
00091     ICM_WriteByte(ICM20948_GYRO_SMPLRT_DIV, gyro_div);
00092     ICM_WriteByte(ICM20948_ACCEL_CONFIG, accel_conf);
00093     ICM_WriteByte(ICM20948_ACCEL_SMPLRT_DIV_2, accel_div);
00094  
00095     // USER_BANK_0 to access data
00096     ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_0);
00097     ThisThread::sleep_for(10ms);
00098 
00099     gyroCalib();
00100 }
00101  
00102 void ICM20948::gyroCalib(){
00103     int16_t gxtmp, gytmp, gztmp;
00104     float gx, gy, gz;
00105     gx = 0.0; gy = 0.0; gz = 0.0;
00106     uint8_t LoByte, HiByte;
00107     for(int i = 0; i < 100; i++){
00108         LoByte = ICM_ReadByte(ICM20948_GYRO_XOUT_L); // read Gyrometer X_Low  value
00109         HiByte = ICM_ReadByte(ICM20948_GYRO_XOUT_H); // read Gyrometer X_High value
00110         gxtmp = ((HiByte<<8) | LoByte);
00111         gx += (float)(gxtmp) * gRes;
00112         
00113         LoByte = ICM_ReadByte(ICM20948_GYRO_YOUT_L);
00114         HiByte = ICM_ReadByte(ICM20948_GYRO_YOUT_H);
00115         gytmp = ((HiByte<<8) | LoByte);
00116         gy += (float)(gytmp) * gRes;
00117         
00118         LoByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_L);
00119         HiByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_H);
00120         gztmp = ((HiByte<<8) | LoByte);
00121         gz += (float)(gztmp) * gRes;
00122         //pc.printf("%d %f, %d %f, %d %f\n\r", gxtmp, gx, gytmp, gy, gztmp, gz);
00123         ThisThread::sleep_for(50ms);
00124     }
00125     
00126     gyroBias[0] = gx / 100.0;
00127     gyroBias[1] = gy / 100.0;
00128     gyroBias[2] = gz / 100.0;
00129 
00130 }
00131  
00132 void ICM20948::getAccGyro(){
00133     int16_t _ax, _ay, _az, _gx, _gy, _gz;
00134     uint8_t LoByte, HiByte;
00135     
00136     LoByte = ICM_ReadByte(ICM20948_ACCEL_XOUT_L); // read Accelerometer X_Low  value
00137     HiByte = ICM_ReadByte(ICM20948_ACCEL_XOUT_H); // read Accelerometer X_High value
00138     _ax = (HiByte<<8) | LoByte;
00139     ax = ((float)(_ax)) * aRes;
00140     
00141     LoByte = ICM_ReadByte(ICM20948_ACCEL_YOUT_L);
00142     HiByte = ICM_ReadByte(ICM20948_ACCEL_YOUT_H);
00143     _ay = (HiByte<<8) | LoByte;
00144     ay = ((float)(_ay)) * aRes;
00145     
00146     LoByte = ICM_ReadByte(ICM20948_ACCEL_ZOUT_L);
00147     HiByte = ICM_ReadByte(ICM20948_ACCEL_ZOUT_H);
00148     _az = (HiByte<<8) | LoByte;
00149     az = ((float)(_az)) * aRes;
00150     
00151     LoByte = ICM_ReadByte(ICM20948_GYRO_XOUT_L); // read Gyrometer X_Low  value
00152     HiByte = ICM_ReadByte(ICM20948_GYRO_XOUT_H); // read Gyrometer X_High value
00153     _gx = (HiByte<<8) | LoByte;
00154     gx = ((float)(_gx)) * gRes - gyroBias[0];
00155     
00156     LoByte = ICM_ReadByte(ICM20948_GYRO_YOUT_L);
00157     HiByte = ICM_ReadByte(ICM20948_GYRO_YOUT_H);
00158     _gy = (HiByte<<8) | LoByte;
00159     gy = ((float)(_gy)) * gRes - gyroBias[1];
00160     
00161     LoByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_L);
00162     HiByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_H);
00163     _gz = (HiByte<<8) | LoByte;
00164     gz = ((float)(_gz)) * gRes - gyroBias[2];
00165     
00166 }
00167 
00168 float ICM20948::getAX()
00169 {
00170     return ax;
00171 }
00172 
00173 float ICM20948::getAY()
00174 {
00175     return ay;
00176 }
00177 
00178 float ICM20948::getAZ()
00179 {
00180     return az;
00181 }
00182 
00183 float ICM20948::getGX()
00184 {
00185     return gx;
00186 }
00187 
00188 float ICM20948::getGY()
00189 {
00190     return gy;
00191 }
00192 
00193 float ICM20948::getGZ()
00194 {
00195     return gz;
00196 }
00197 
00198 int ICM20948::getIMUTemp()
00199 {
00200     uint8_t LoByte, HiByte;
00201     LoByte = ICM_ReadByte(ICM20948_TEMP_OUT_L); // read Accelerometer X_Low  value
00202     HiByte = ICM_ReadByte(ICM20948_TEMP_OUT_H); // read Accelerometer X_High value
00203     return ((HiByte<<8) | LoByte);
00204 }
00205