BMX055.cpp

00001 #include "mbed.h"
00002 #include "BMX055.h"
00003 BMX055::BMX055(PinName SDA, PinName SCL) : bmx055(SDA,SCL){
00004     bmx055.frequency(100000);
00005     bmx_init();
00006 }
00007 void BMX055::bmx_init(void)
00008 {
00009     char buf[2]={0};
00010     
00011     printf("magnet setting\r\n");
00012     buf[0]=0x4B; buf[1]=0x82;
00013     bmx055.write(MAG,buf,2); wait(0.1);
00014     buf[0]=0x4B; buf[1]=0x01;
00015     bmx055.write(MAG,buf,2); wait(0.1);
00016     buf[0]=0x4C; buf[1]=0x00;
00017     bmx055.write(MAG,buf,2);
00018     buf[0]=0x4E; buf[1]=0x84;
00019     bmx055.write(MAG,buf,2);
00020     buf[0]=0x51; buf[1]=0x04;
00021     bmx055.write(MAG,buf,2);
00022     buf[0]=0x52; buf[1]=0x16;
00023     bmx055.write(MAG,buf,2);
00024     wait(0.1);
00025     buf[0]=0x00;
00026     bmx055.write(MAG,buf,1,1);
00027     bmx055.read(MAG,buf,1);
00028     printf("read:0x%02x\r\n",buf[0]);
00029 }
00030  void BMX055::getAcc(void){
00031     uint8_t data[6]={0};
00032     char send[1], get[1];
00033     char temp;
00034     send[0]=(char)(2);
00035     bmx055.write(ACC,send,1,true);
00036     bmx055.read(ACC,(char*)data,6);
00037     for(int i=0;i<3;i++){
00038         accel[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 4;
00039         if(accel[i]>2047)accel[i]-=4096;
00040     }
00041 }
00042 /*
00043 void BMX055::getAcc(void)
00044 {
00045   unsigned int data[6];
00046   for (int i = 0; i < 6; i++)
00047   {
00048     Wire.beginTransmission(Addr_Accl);
00049     Wire.write((2 + i));// Select data register
00050     Wire.endTransmission();
00051     Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
00052     // Read 6 bytes of data
00053     // xAccl lsb, xAccl msb, yAccl lsb, yAccl msb, zAccl lsb, zAccl msb
00054     if (Wire.available() == 1)
00055       data[i] = Wire.read();
00056   }
00057   // Convert the data to 12-bits
00058   xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
00059   if (xAccl > 2047)  xAccl -= 4096;
00060   yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
00061   if (yAccl > 2047)  yAccl -= 4096;
00062   zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
00063   if (zAccl > 2047)  zAccl -= 4096;
00064   xAccl = xAccl * 0.0098; // range = +/-2g
00065   yAccl = yAccl * 0.0098; // range = +/-2g
00066   zAccl = zAccl * 0.0098; // range = +/-2g
00067 }
00068 */
00069 void BMX055::getGyro(void){
00070     int data[6]={0};
00071     char send[1],get[1];
00072     char temp;
00073     for(int i=0;i<6;i++){
00074         send[0]=(char)(2+i);
00075         bmx055.write(GYRO,send,1);
00076         bmx055.read(GYRO,get,1);
00077         temp=get[0];
00078         data[i]=temp;
00079     }
00080     for(int i=0;i<3;i++){
00081         gyroscope[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 4;
00082         if(gyroscope[i]>32767)gyroscope[i]-=65536;
00083 //        gyroscope[i]=gyroscope[i]*0.0038;
00084     }
00085 }
00086 void BMX055::getMag(void){
00087     int data[8]={0};
00088     char send[1],get[1];
00089     char temp;
00090     for(int i=0;i<8;i++){
00091         send[0]=(char)(0x42+i);
00092         bmx055.write(MAG,send,1);
00093         bmx055.read(MAG,get,1);
00094 //        printf(“%02X “,get[0]);
00095         temp=get[0];
00096         data[i]=temp;
00097     }
00098     for(int i=0;i<3;i++){
00099         if(i!=2)magnet[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 3;
00100         else magnet[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 1;
00101         if(i==2 && magnet[i]>16383)magnet[i]-=32768;
00102         else if(i!=2 && magnet[i]>4095)magnet[i]-=8092;
00103     }
00104 }