Diff: I2CDriverTest03.h

Revision:

7:04824382eafb

Child:

13:530968937ccb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I2CDriverTest03.h	Sun Apr 28 15:08:04 2013 +0000
@@ -0,0 +1,173 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "I2CMasterRtos.h"
+#include "stdint.h"
+
+const int dataReadySig = 1<<5;
+osThreadId mainThreadID = 0;
+char data[64];
+int16_t fifo[16];
+const int i2cAdr = 0x68<<1;
+int fifoAdr = 0x72;
+
+//Serial pc(USBTX, USBRX);
+
+void configMPU6050(I2CMasterRtos& i2c);
+void config(I2CMasterRtos& i2c);
+
+
+void dataReadyIsr()
+{
+    osSignalSet(mainThreadID, dataReadySig);
+}
+
+void readModWrite(I2CMasterRtos& i2c, uint8_t reg, uint8_t dta)
+{
+    
+    char rd1;
+    int rStat1 = i2c.read(i2cAdr, reg, &rd1, 1);
+    char data[2];
+    data[0]=(char)reg;
+    data[1]=(char)dta;
+    char rd2;
+    int wStat = i2c.write(i2cAdr, data, 2);
+    osDelay(500);
+    int rStat2 = i2c.read(i2cAdr, reg, &rd2, 1);
+    printf("%2d%2d%2d  %2x <- %2x  => %2x -> %2x \n", rStat1, wStat, rStat2, reg, dta, rd1, rd2);
+}
+
+
+int doit()
+{
+    //pc.baud(115200);
+    mainThreadID = osThreadGetId();
+
+    I2CMasterRtos i2c(p28, p27,400000);
+    osDelay(500);
+
+    printf("Initialize ... \n");
+    config(i2c);
+
+    printf("Action!\n");
+
+    InterruptIn dataReadyIrq(p8);
+    dataReadyIrq.mode(PullNone);
+    dataReadyIrq.rise(&dataReadyIsr);
+
+    /*
+    data[0]=0x6a;   // pwr 1 reg
+    data[1]=(1<<6)|(1<<2); // fifo on
+    i2c.write(i2cAdr,data,2,1);
+
+    data[0]=0x38;   // irq conf  reg
+    data[1]=1;      // irq on data ready
+    i2c.write(i2cAdr,data,2,1);
+    */
+    //fifoAdr = 0x3b;
+    char devNull;
+    while(1) {
+        osSignalWait(dataReadySig, 1000); // osWaitForever
+        i2c.read(i2cAdr,fifoAdr,data,2);
+        i2c.read(i2cAdr,fifoAdr+2,data+2,12);
+        i2c.read(i2cAdr,0x3a,&devNull,1);
+        for(int i=0; i<7; i++) {
+            fifo[i] = (data[2*i]<<8) | data[2*i+1];
+            printf("%8d",fifo[i]);
+        }
+        printf("     %x\n",devNull);
+
+    }
+    return 0;
+}
+
+static void config(I2CMasterRtos& i2c)
+{
+    uint8_t ncfg=32;
+    uint8_t regs[ncfg];
+    uint8_t vals[ncfg];
+    int cnt=0;
+    regs[cnt]=0x6b;
+    vals[cnt++]=(1<<7); // pwr 1 reg //: device reset
+    regs[cnt]=0x6b;
+    vals[cnt++]=1; // pwr 1 reg // clock from x gyro all pwr sav modes off
+    regs[cnt]=0x19;
+    vals[cnt++]=199;  // sample rate divider reg  // sapmle rate = gyro rate / (1+x)
+    regs[cnt]=0x1a;
+    vals[cnt++]=1;// conf  reg // no ext frame sync / dig low pass set to 1 => 1kHz Sampling with ~200Hz bandwidth DLPF
+    regs[cnt]=0x1b;
+    vals[cnt++]=0;// gyro conf  reg // no test mode and gyro range 250°/s
+    regs[cnt]=0x1c;
+    vals[cnt++]=0;// accl conf  reg // no test mode and accl range 2g
+    regs[cnt]=0x23;
+    vals[cnt++]=0xf<<3;// fifo conf  reg // accl + all gyro -> fifo
+    regs[cnt]=0x37;
+    vals[cnt++]=(0<<7)|(0<<6)|(0<<5)|(0<<4); // irq conf  reg // act high | 0:pupu 1:opnDrn| pulse | clear on any read
+    regs[cnt]=0x38;
+    vals[cnt++]=1|(1<<4); // irq conf  reg // irq on data ready
+    regs[cnt]=0x6a;
+    vals[cnt++]=(1<<2); // pwr 1 reg // fifo reset
+    regs[cnt]=0x6a;
+    vals[cnt++]=(1<<6); // pwr 1 reg // fifo on
+
+    /*
+    readModWrite(i2c, regs[0], vals[0]);
+    char reset=0xff;
+    while(reset&(1<<7)) {
+        osDelay(100);
+        i2c.read(i2cAdr,0x6b,&reset,1,1);
+    }
+    */
+    for(int i=0; i<cnt; i++)
+        readModWrite(i2c, regs[i], vals[i]);
+}
+
+static void configMPU6050(I2CMasterRtos& i2c)
+{
+
+    data[0]=0x6b;   // pwr 1 reg
+    data[1]=1<<7;   // device reset
+    i2c.write(i2cAdr,data,2,1);
+    char reset=0xff;
+    while(reset&(1<<7)) {
+        osDelay(100);
+        i2c.read(i2cAdr,0x6b,&reset,1,1);
+    }
+
+    data[0]=0x19;   // sample rate divider reg
+    data[1]=99;    // sapmle rate = gyro rate / (1+x)
+    i2c.write(i2cAdr,data,2,1);
+
+    data[0]=0x1a;   // conf  reg
+    data[1]=1;      // no ext frame sync / dig low pass set to 1 => 1kHz Sampling with ~200Hz bandwidth DLPF
+    i2c.write(i2cAdr,data,2,1);
+
+    data[0]=0x1b;   // gyro conf  reg
+    data[1]=0;      // no test mode and gyro range 250°/s
+    i2c.write(i2cAdr,data,2,1);
+
+    data[0]=0x1c;   // accl conf  reg
+    data[1]=0;      // no test mode and accl range 2g
+    i2c.write(i2cAdr,data,2,1);
+
+    data[0]=0x23;   // fifo conf  reg
+    data[1]=0xf<<3; // accl + all gyro -> fifo
+    i2c.write(i2cAdr,data,2,1);
+
+    data[0]=0x37;   // irq conf  reg
+    data[1]=(1<<7)|(0<<6)|(0<<5)|(1<<4); // act high | pupu | pulse | clear on any read
+    i2c.write(i2cAdr,data,2,1);
+
+    /*
+    data[0]=0x38;   // irq conf  reg
+    data[1]=1;      // irq on data ready
+    i2c.write(i2cAdr,data,2,1);
+
+    data[0]=0x6a;   // pwr 1 reg
+    data[1]=(1<<6); // fifo on
+    i2c.write(i2cAdr,data,2,1);
+    */
+    data[0]=0x6b;   // pwr 1 reg
+    data[1]=1;      // clock from x gyro all pwr sav modes off
+    i2c.write(i2cAdr,data,2,1);
+}
+