I2CDriverTest03.h

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "I2CMasterRtos.h"
00004 #include "stdint.h"
00005 
00006 const int dataReadySig = 1<<5;
00007 osThreadId mainThreadID = 0;
00008 char data[64];
00009 int16_t fifo[16];
00010 const int i2cAdr = 0x68<<1;
00011 int fifoAdr = 0x72;
00012 
00013 //Serial pc(USBTX, USBRX);
00014 
00015 void configMPU6050(I2CMasterRtos& i2c);
00016 void config(I2CMasterRtos& i2c);
00017 
00018 
00019 void dataReadyIsr()
00020 {
00021     osSignalSet(mainThreadID, dataReadySig);
00022 }
00023 
00024 void readModWrite(I2CMasterRtos& i2c, uint8_t reg, uint8_t dta)
00025 {
00026     
00027     char rd1;
00028     int rStat1 = i2c.read(i2cAdr, reg, &rd1, 1);
00029     char data[2];
00030     data[0]=(char)reg;
00031     data[1]=(char)dta;
00032     char rd2;
00033     int wStat = i2c.write(i2cAdr, data, 2);
00034     osDelay(500);
00035     int rStat2 = i2c.read(i2cAdr, reg, &rd2, 1);
00036     printf("%2d%2d%2d  %2x <- %2x  => %2x -> %2x \n", rStat1, wStat, rStat2, reg, dta, rd1, rd2);
00037 }
00038 
00039 
00040 int doit()
00041 {
00042     //pc.baud(115200);
00043     mainThreadID = osThreadGetId();
00044 
00045     I2CMasterRtos i2c(p28, p27,400000);
00046     osDelay(500);
00047 
00048     printf("Initialize ... \n");
00049     config(i2c);
00050 
00051     printf("Action!\n");
00052 
00053     InterruptIn dataReadyIrq(p8);
00054     dataReadyIrq.mode(PullNone);
00055     dataReadyIrq.rise(&dataReadyIsr);
00056 
00057     /*
00058     data[0]=0x6a;   // pwr 1 reg
00059     data[1]=(1<<6)|(1<<2); // fifo on
00060     i2c.write(i2cAdr,data,2,1);
00061 
00062     data[0]=0x38;   // irq conf  reg
00063     data[1]=1;      // irq on data ready
00064     i2c.write(i2cAdr,data,2,1);
00065     */
00066     //fifoAdr = 0x3b;
00067     char devNull;
00068     while(1) {
00069         osSignalWait(dataReadySig, 1000); // osWaitForever
00070         i2c.read(i2cAdr,fifoAdr,data,2);
00071         i2c.read(i2cAdr,fifoAdr+2,data+2,12);
00072         i2c.read(i2cAdr,0x3a,&devNull,1);
00073         for(int i=0; i<7; i++) {
00074             fifo[i] = (data[2*i]<<8) | data[2*i+1];
00075             printf("%8d",fifo[i]);
00076         }
00077         printf("     %x\n",devNull);
00078 
00079     }
00080     return 0;
00081 }
00082 
00083 static void config(I2CMasterRtos& i2c)
00084 {
00085     uint8_t ncfg=32;
00086     uint8_t regs[ncfg];
00087     uint8_t vals[ncfg];
00088     int cnt=0;
00089     regs[cnt]=0x6b;
00090     vals[cnt++]=(1<<7); // pwr 1 reg //: device reset
00091     regs[cnt]=0x6b;
00092     vals[cnt++]=1; // pwr 1 reg // clock from x gyro all pwr sav modes off
00093     regs[cnt]=0x19;
00094     vals[cnt++]=199;  // sample rate divider reg  // sapmle rate = gyro rate / (1+x)
00095     regs[cnt]=0x1a;
00096     vals[cnt++]=1;// conf  reg // no ext frame sync / dig low pass set to 1 => 1kHz Sampling with ~200Hz bandwidth DLPF
00097     regs[cnt]=0x1b;
00098     vals[cnt++]=0;// gyro conf  reg // no test mode and gyro range 250°/s
00099     regs[cnt]=0x1c;
00100     vals[cnt++]=0;// accl conf  reg // no test mode and accl range 2g
00101     regs[cnt]=0x23;
00102     vals[cnt++]=0xf<<3;// fifo conf  reg // accl + all gyro -> fifo
00103     regs[cnt]=0x37;
00104     vals[cnt++]=(0<<7)|(0<<6)|(0<<5)|(0<<4); // irq conf  reg // act high | 0:pupu 1:opnDrn| pulse | clear on any read
00105     regs[cnt]=0x38;
00106     vals[cnt++]=1|(1<<4); // irq conf  reg // irq on data ready
00107     regs[cnt]=0x6a;
00108     vals[cnt++]=(1<<2); // pwr 1 reg // fifo reset
00109     regs[cnt]=0x6a;
00110     vals[cnt++]=(1<<6); // pwr 1 reg // fifo on
00111 
00112     /*
00113     readModWrite(i2c, regs[0], vals[0]);
00114     char reset=0xff;
00115     while(reset&(1<<7)) {
00116         osDelay(100);
00117         i2c.read(i2cAdr,0x6b,&reset,1,1);
00118     }
00119     */
00120     for(int i=0; i<cnt; i++)
00121         readModWrite(i2c, regs[i], vals[i]);
00122 }
00123 
00124 static void configMPU6050(I2CMasterRtos& i2c)
00125 {
00126 
00127     data[0]=0x6b;   // pwr 1 reg
00128     data[1]=1<<7;   // device reset
00129     i2c.write(i2cAdr,data,2,1);
00130     char reset=0xff;
00131     while(reset&(1<<7)) {
00132         osDelay(100);
00133         i2c.read(i2cAdr,0x6b,&reset,1,1);
00134     }
00135 
00136     data[0]=0x19;   // sample rate divider reg
00137     data[1]=99;    // sapmle rate = gyro rate / (1+x)
00138     i2c.write(i2cAdr,data,2,1);
00139 
00140     data[0]=0x1a;   // conf  reg
00141     data[1]=1;      // no ext frame sync / dig low pass set to 1 => 1kHz Sampling with ~200Hz bandwidth DLPF
00142     i2c.write(i2cAdr,data,2,1);
00143 
00144     data[0]=0x1b;   // gyro conf  reg
00145     data[1]=0;      // no test mode and gyro range 250°/s
00146     i2c.write(i2cAdr,data,2,1);
00147 
00148     data[0]=0x1c;   // accl conf  reg
00149     data[1]=0;      // no test mode and accl range 2g
00150     i2c.write(i2cAdr,data,2,1);
00151 
00152     data[0]=0x23;   // fifo conf  reg
00153     data[1]=0xf<<3; // accl + all gyro -> fifo
00154     i2c.write(i2cAdr,data,2,1);
00155 
00156     data[0]=0x37;   // irq conf  reg
00157     data[1]=(1<<7)|(0<<6)|(0<<5)|(1<<4); // act high | pupu | pulse | clear on any read
00158     i2c.write(i2cAdr,data,2,1);
00159 
00160     /*
00161     data[0]=0x38;   // irq conf  reg
00162     data[1]=1;      // irq on data ready
00163     i2c.write(i2cAdr,data,2,1);
00164 
00165     data[0]=0x6a;   // pwr 1 reg
00166     data[1]=(1<<6); // fifo on
00167     i2c.write(i2cAdr,data,2,1);
00168     */
00169     data[0]=0x6b;   // pwr 1 reg
00170     data[1]=1;      // clock from x gyro all pwr sav modes off
00171     i2c.write(i2cAdr,data,2,1);
00172 }
00173