I2CRTOS Driver by Helmut Schmücker. Removed included mbed-rtos library to prevent multiple definition. Make sure to include mbed-rtos library in your program!
Fork of I2cRtosDriver by
I2CDriverTest03.h@7:04824382eafb, 2013-04-28 (annotated)
- Committer:
- humlet
- Date:
- Sun Apr 28 15:08:04 2013 +0000
- Revision:
- 7:04824382eafb
- Child:
- 13:530968937ccb
stable before perf measurements
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
humlet | 7:04824382eafb | 1 | #include "mbed.h" |
humlet | 7:04824382eafb | 2 | #include "rtos.h" |
humlet | 7:04824382eafb | 3 | #include "I2CMasterRtos.h" |
humlet | 7:04824382eafb | 4 | #include "stdint.h" |
humlet | 7:04824382eafb | 5 | |
humlet | 7:04824382eafb | 6 | const int dataReadySig = 1<<5; |
humlet | 7:04824382eafb | 7 | osThreadId mainThreadID = 0; |
humlet | 7:04824382eafb | 8 | char data[64]; |
humlet | 7:04824382eafb | 9 | int16_t fifo[16]; |
humlet | 7:04824382eafb | 10 | const int i2cAdr = 0x68<<1; |
humlet | 7:04824382eafb | 11 | int fifoAdr = 0x72; |
humlet | 7:04824382eafb | 12 | |
humlet | 7:04824382eafb | 13 | //Serial pc(USBTX, USBRX); |
humlet | 7:04824382eafb | 14 | |
humlet | 7:04824382eafb | 15 | void configMPU6050(I2CMasterRtos& i2c); |
humlet | 7:04824382eafb | 16 | void config(I2CMasterRtos& i2c); |
humlet | 7:04824382eafb | 17 | |
humlet | 7:04824382eafb | 18 | |
humlet | 7:04824382eafb | 19 | void dataReadyIsr() |
humlet | 7:04824382eafb | 20 | { |
humlet | 7:04824382eafb | 21 | osSignalSet(mainThreadID, dataReadySig); |
humlet | 7:04824382eafb | 22 | } |
humlet | 7:04824382eafb | 23 | |
humlet | 7:04824382eafb | 24 | void readModWrite(I2CMasterRtos& i2c, uint8_t reg, uint8_t dta) |
humlet | 7:04824382eafb | 25 | { |
humlet | 7:04824382eafb | 26 | |
humlet | 7:04824382eafb | 27 | char rd1; |
humlet | 7:04824382eafb | 28 | int rStat1 = i2c.read(i2cAdr, reg, &rd1, 1); |
humlet | 7:04824382eafb | 29 | char data[2]; |
humlet | 7:04824382eafb | 30 | data[0]=(char)reg; |
humlet | 7:04824382eafb | 31 | data[1]=(char)dta; |
humlet | 7:04824382eafb | 32 | char rd2; |
humlet | 7:04824382eafb | 33 | int wStat = i2c.write(i2cAdr, data, 2); |
humlet | 7:04824382eafb | 34 | osDelay(500); |
humlet | 7:04824382eafb | 35 | int rStat2 = i2c.read(i2cAdr, reg, &rd2, 1); |
humlet | 7:04824382eafb | 36 | printf("%2d%2d%2d %2x <- %2x => %2x -> %2x \n", rStat1, wStat, rStat2, reg, dta, rd1, rd2); |
humlet | 7:04824382eafb | 37 | } |
humlet | 7:04824382eafb | 38 | |
humlet | 7:04824382eafb | 39 | |
humlet | 7:04824382eafb | 40 | int doit() |
humlet | 7:04824382eafb | 41 | { |
humlet | 7:04824382eafb | 42 | //pc.baud(115200); |
humlet | 7:04824382eafb | 43 | mainThreadID = osThreadGetId(); |
humlet | 7:04824382eafb | 44 | |
humlet | 7:04824382eafb | 45 | I2CMasterRtos i2c(p28, p27,400000); |
humlet | 7:04824382eafb | 46 | osDelay(500); |
humlet | 7:04824382eafb | 47 | |
humlet | 7:04824382eafb | 48 | printf("Initialize ... \n"); |
humlet | 7:04824382eafb | 49 | config(i2c); |
humlet | 7:04824382eafb | 50 | |
humlet | 7:04824382eafb | 51 | printf("Action!\n"); |
humlet | 7:04824382eafb | 52 | |
humlet | 7:04824382eafb | 53 | InterruptIn dataReadyIrq(p8); |
humlet | 7:04824382eafb | 54 | dataReadyIrq.mode(PullNone); |
humlet | 7:04824382eafb | 55 | dataReadyIrq.rise(&dataReadyIsr); |
humlet | 7:04824382eafb | 56 | |
humlet | 7:04824382eafb | 57 | /* |
humlet | 7:04824382eafb | 58 | data[0]=0x6a; // pwr 1 reg |
humlet | 7:04824382eafb | 59 | data[1]=(1<<6)|(1<<2); // fifo on |
humlet | 7:04824382eafb | 60 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 61 | |
humlet | 7:04824382eafb | 62 | data[0]=0x38; // irq conf reg |
humlet | 7:04824382eafb | 63 | data[1]=1; // irq on data ready |
humlet | 7:04824382eafb | 64 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 65 | */ |
humlet | 7:04824382eafb | 66 | //fifoAdr = 0x3b; |
humlet | 7:04824382eafb | 67 | char devNull; |
humlet | 7:04824382eafb | 68 | while(1) { |
humlet | 7:04824382eafb | 69 | osSignalWait(dataReadySig, 1000); // osWaitForever |
humlet | 7:04824382eafb | 70 | i2c.read(i2cAdr,fifoAdr,data,2); |
humlet | 7:04824382eafb | 71 | i2c.read(i2cAdr,fifoAdr+2,data+2,12); |
humlet | 7:04824382eafb | 72 | i2c.read(i2cAdr,0x3a,&devNull,1); |
humlet | 7:04824382eafb | 73 | for(int i=0; i<7; i++) { |
humlet | 7:04824382eafb | 74 | fifo[i] = (data[2*i]<<8) | data[2*i+1]; |
humlet | 7:04824382eafb | 75 | printf("%8d",fifo[i]); |
humlet | 7:04824382eafb | 76 | } |
humlet | 7:04824382eafb | 77 | printf(" %x\n",devNull); |
humlet | 7:04824382eafb | 78 | |
humlet | 7:04824382eafb | 79 | } |
humlet | 7:04824382eafb | 80 | return 0; |
humlet | 7:04824382eafb | 81 | } |
humlet | 7:04824382eafb | 82 | |
humlet | 7:04824382eafb | 83 | static void config(I2CMasterRtos& i2c) |
humlet | 7:04824382eafb | 84 | { |
humlet | 7:04824382eafb | 85 | uint8_t ncfg=32; |
humlet | 7:04824382eafb | 86 | uint8_t regs[ncfg]; |
humlet | 7:04824382eafb | 87 | uint8_t vals[ncfg]; |
humlet | 7:04824382eafb | 88 | int cnt=0; |
humlet | 7:04824382eafb | 89 | regs[cnt]=0x6b; |
humlet | 7:04824382eafb | 90 | vals[cnt++]=(1<<7); // pwr 1 reg //: device reset |
humlet | 7:04824382eafb | 91 | regs[cnt]=0x6b; |
humlet | 7:04824382eafb | 92 | vals[cnt++]=1; // pwr 1 reg // clock from x gyro all pwr sav modes off |
humlet | 7:04824382eafb | 93 | regs[cnt]=0x19; |
humlet | 7:04824382eafb | 94 | vals[cnt++]=199; // sample rate divider reg // sapmle rate = gyro rate / (1+x) |
humlet | 7:04824382eafb | 95 | regs[cnt]=0x1a; |
humlet | 7:04824382eafb | 96 | vals[cnt++]=1;// conf reg // no ext frame sync / dig low pass set to 1 => 1kHz Sampling with ~200Hz bandwidth DLPF |
humlet | 7:04824382eafb | 97 | regs[cnt]=0x1b; |
humlet | 7:04824382eafb | 98 | vals[cnt++]=0;// gyro conf reg // no test mode and gyro range 250°/s |
humlet | 7:04824382eafb | 99 | regs[cnt]=0x1c; |
humlet | 7:04824382eafb | 100 | vals[cnt++]=0;// accl conf reg // no test mode and accl range 2g |
humlet | 7:04824382eafb | 101 | regs[cnt]=0x23; |
humlet | 7:04824382eafb | 102 | vals[cnt++]=0xf<<3;// fifo conf reg // accl + all gyro -> fifo |
humlet | 7:04824382eafb | 103 | regs[cnt]=0x37; |
humlet | 7:04824382eafb | 104 | vals[cnt++]=(0<<7)|(0<<6)|(0<<5)|(0<<4); // irq conf reg // act high | 0:pupu 1:opnDrn| pulse | clear on any read |
humlet | 7:04824382eafb | 105 | regs[cnt]=0x38; |
humlet | 7:04824382eafb | 106 | vals[cnt++]=1|(1<<4); // irq conf reg // irq on data ready |
humlet | 7:04824382eafb | 107 | regs[cnt]=0x6a; |
humlet | 7:04824382eafb | 108 | vals[cnt++]=(1<<2); // pwr 1 reg // fifo reset |
humlet | 7:04824382eafb | 109 | regs[cnt]=0x6a; |
humlet | 7:04824382eafb | 110 | vals[cnt++]=(1<<6); // pwr 1 reg // fifo on |
humlet | 7:04824382eafb | 111 | |
humlet | 7:04824382eafb | 112 | /* |
humlet | 7:04824382eafb | 113 | readModWrite(i2c, regs[0], vals[0]); |
humlet | 7:04824382eafb | 114 | char reset=0xff; |
humlet | 7:04824382eafb | 115 | while(reset&(1<<7)) { |
humlet | 7:04824382eafb | 116 | osDelay(100); |
humlet | 7:04824382eafb | 117 | i2c.read(i2cAdr,0x6b,&reset,1,1); |
humlet | 7:04824382eafb | 118 | } |
humlet | 7:04824382eafb | 119 | */ |
humlet | 7:04824382eafb | 120 | for(int i=0; i<cnt; i++) |
humlet | 7:04824382eafb | 121 | readModWrite(i2c, regs[i], vals[i]); |
humlet | 7:04824382eafb | 122 | } |
humlet | 7:04824382eafb | 123 | |
humlet | 7:04824382eafb | 124 | static void configMPU6050(I2CMasterRtos& i2c) |
humlet | 7:04824382eafb | 125 | { |
humlet | 7:04824382eafb | 126 | |
humlet | 7:04824382eafb | 127 | data[0]=0x6b; // pwr 1 reg |
humlet | 7:04824382eafb | 128 | data[1]=1<<7; // device reset |
humlet | 7:04824382eafb | 129 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 130 | char reset=0xff; |
humlet | 7:04824382eafb | 131 | while(reset&(1<<7)) { |
humlet | 7:04824382eafb | 132 | osDelay(100); |
humlet | 7:04824382eafb | 133 | i2c.read(i2cAdr,0x6b,&reset,1,1); |
humlet | 7:04824382eafb | 134 | } |
humlet | 7:04824382eafb | 135 | |
humlet | 7:04824382eafb | 136 | data[0]=0x19; // sample rate divider reg |
humlet | 7:04824382eafb | 137 | data[1]=99; // sapmle rate = gyro rate / (1+x) |
humlet | 7:04824382eafb | 138 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 139 | |
humlet | 7:04824382eafb | 140 | data[0]=0x1a; // conf reg |
humlet | 7:04824382eafb | 141 | data[1]=1; // no ext frame sync / dig low pass set to 1 => 1kHz Sampling with ~200Hz bandwidth DLPF |
humlet | 7:04824382eafb | 142 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 143 | |
humlet | 7:04824382eafb | 144 | data[0]=0x1b; // gyro conf reg |
humlet | 7:04824382eafb | 145 | data[1]=0; // no test mode and gyro range 250°/s |
humlet | 7:04824382eafb | 146 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 147 | |
humlet | 7:04824382eafb | 148 | data[0]=0x1c; // accl conf reg |
humlet | 7:04824382eafb | 149 | data[1]=0; // no test mode and accl range 2g |
humlet | 7:04824382eafb | 150 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 151 | |
humlet | 7:04824382eafb | 152 | data[0]=0x23; // fifo conf reg |
humlet | 7:04824382eafb | 153 | data[1]=0xf<<3; // accl + all gyro -> fifo |
humlet | 7:04824382eafb | 154 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 155 | |
humlet | 7:04824382eafb | 156 | data[0]=0x37; // irq conf reg |
humlet | 7:04824382eafb | 157 | data[1]=(1<<7)|(0<<6)|(0<<5)|(1<<4); // act high | pupu | pulse | clear on any read |
humlet | 7:04824382eafb | 158 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 159 | |
humlet | 7:04824382eafb | 160 | /* |
humlet | 7:04824382eafb | 161 | data[0]=0x38; // irq conf reg |
humlet | 7:04824382eafb | 162 | data[1]=1; // irq on data ready |
humlet | 7:04824382eafb | 163 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 164 | |
humlet | 7:04824382eafb | 165 | data[0]=0x6a; // pwr 1 reg |
humlet | 7:04824382eafb | 166 | data[1]=(1<<6); // fifo on |
humlet | 7:04824382eafb | 167 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 168 | */ |
humlet | 7:04824382eafb | 169 | data[0]=0x6b; // pwr 1 reg |
humlet | 7:04824382eafb | 170 | data[1]=1; // clock from x gyro all pwr sav modes off |
humlet | 7:04824382eafb | 171 | i2c.write(i2cAdr,data,2,1); |
humlet | 7:04824382eafb | 172 | } |
humlet | 7:04824382eafb | 173 |