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
|
|