I2CDriverTest05.h

00001 // several threads try to read concurrently from a MPU6050 gyro/acc meter
00002 // via the same globally defined i2c driver interface
00003 
00004 #include "mbed.h"
00005 #include "rtos.h"
00006 #include "I2CMasterRtos.h"
00007 #include "stdint.h"
00008 
00009 const uint32_t i2cAdr = 0x68<<1;
00010 const char reg= 0x3b;  // accelerometer x,y,z
00011 volatile osThreadId i2cDrvThrdID[2];
00012 
00013 I2CMasterRtos g_i2c(p28, p27, 400000);
00014 
00015 static void config(I2CMasterRtos& i2c);
00016 
00017 void highPrioCallBck(void const *args)
00018 {
00019     osSignalSet(i2cDrvThrdID[1], 1<<5);
00020     osSignalSet(i2cDrvThrdID[0], 1<<5);
00021 }
00022 
00023 void highPrioThreadFun(void const *args)
00024 {
00025     int thrdID = (int)args;
00026     i2cDrvThrdID[thrdID] = Thread::gettid();
00027 
00028     char result[64];
00029     while(true) {
00030         Thread::signal_wait(1<<5,osWaitForever);
00031         g_i2c.lock();
00032         g_i2c.read(i2cAdr, reg, result, 3*2);
00033         printf("%s prio thread has read from MPU650:", (thrdID==0?"high       ":"even higher"));
00034         for(int i=0; i<3; i++) {
00035             int16_t acc=((static_cast<int16_t>(result[i*2])<<8)|static_cast<int16_t>(result[i*2+1]));
00036             printf("%7i",acc);
00037         }
00038         printf("\n");
00039         g_i2c.unlock();
00040     }
00041 }
00042 
00043 int doit()
00044 {
00045     I2CMasterRtos i2c(p28, p27, 100000);
00046     config(i2c);
00047 
00048     Thread highPrioThread(highPrioThreadFun, 0, osPriorityAboveNormal);
00049     Thread evenHigherPrioThread(highPrioThreadFun, (void*)1, osPriorityHigh);
00050     RtosTimer highPrioTicker(highPrioCallBck, osTimerPeriodic, (void *)0);
00051 
00052     Thread::wait(1000);
00053     highPrioTicker.start(503);
00054 
00055     char result[64];
00056     for(int i=0; i<100; ++i) {
00057         i2c.read(i2cAdr, reg, result, 3*2);
00058         printf("normal      prio thread has read from MPU650:");
00059         for(int i=0; i<3; i++) {
00060             int16_t acc=((static_cast<int16_t>(result[i*2])<<8)|static_cast<int16_t>(result[i*2+1]));
00061             printf("%7i",acc);
00062         }
00063         printf("\n");
00064         Thread::wait(100);
00065     }
00066     return 0;
00067 }
00068 
00069 void readModWrite(I2CMasterRtos& i2c, uint8_t reg, uint8_t dta)
00070 {
00071     char rd1;
00072     int rStat1 = i2c.read(i2cAdr, reg, &rd1,1);
00073     char data[2];
00074     data[0]=(char)reg;
00075     data[1]=(char)dta;
00076     char rd2;
00077     int wStat = i2c.write(i2cAdr, data, 2);
00078     osDelay(100);
00079     int rStat2 = i2c.read(i2cAdr, reg, &rd2,1);
00080     printf("(%3x%3x%3x)  %2x <- %2x  => %2x -> %2x \n", rStat1, wStat, rStat2, reg, dta, rd1, rd2);
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++]=0;  // sample rate divider reg  // sapmle rate = gyro rate / (1+x)
00095     regs[cnt]=0x1a;
00096     vals[cnt++]=0x01;// conf  reg // no ext frame sync / no dig low pass set to 1 => 1kHz Sampling
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++]=0x1f<<3;// fifo conf  reg // accl + all gyro -> fifo
00103     //regs[cnt]=0x6a;
00104     //vals[cnt++]=(1<<2); // pwr 1 reg // fifo reset
00105     //regs[cnt]=0x6a;
00106     //vals[cnt++]=(1<<6); // pwr 1 reg // fifo on
00107 
00108     for(int i=0; i<cnt; i++)
00109         readModWrite(i2c, regs[i], vals[i]);
00110 }