User | Revision | Line number | New contents of line |
humlet |
14:352609d395c1
|
1
|
// A high prio thread reads at a rate of 1kHz from a MPU6050 gyro/acc meter's FIFO
|
humlet |
14:352609d395c1
|
2
|
// data packets of different size, whereas the lower prio ain thread the CPU time left.
|
humlet |
14:352609d395c1
|
3
|
|
humlet |
7:04824382eafb
|
4
|
#include "mbed.h"
|
humlet |
7:04824382eafb
|
5
|
#include "rtos.h"
|
humlet |
7:04824382eafb
|
6
|
#include "I2CMasterRtos.h"
|
humlet |
7:04824382eafb
|
7
|
#include "stdint.h"
|
humlet |
7:04824382eafb
|
8
|
|
humlet |
14:352609d395c1
|
9
|
//#include "DigitalOut.h"
|
humlet |
14:352609d395c1
|
10
|
//DigitalOut osci(p8);
|
humlet |
10:e3d6c92ff222
|
11
|
|
humlet |
7:04824382eafb
|
12
|
volatile int g_disco=0;
|
humlet |
7:04824382eafb
|
13
|
volatile int g_len=0;
|
humlet |
7:04824382eafb
|
14
|
volatile int g_freq=100000;
|
humlet |
7:04824382eafb
|
15
|
const uint32_t i2cAdr = 0x68<<1;
|
humlet |
7:04824382eafb
|
16
|
|
humlet |
7:04824382eafb
|
17
|
static void config(I2CMasterRtos& i2c);
|
humlet |
7:04824382eafb
|
18
|
|
humlet |
14:352609d395c1
|
19
|
I2CMasterRtos i2c(p28, p27);
|
humlet |
14:352609d395c1
|
20
|
|
humlet |
7:04824382eafb
|
21
|
void highPrioCallBck(void const *args)
|
humlet |
7:04824382eafb
|
22
|
{
|
humlet |
14:352609d395c1
|
23
|
//I2CDriver::osci2.write(0);
|
humlet |
14:352609d395c1
|
24
|
const char reg= 0x74;
|
humlet |
14:352609d395c1
|
25
|
static char result[64];
|
humlet |
14:352609d395c1
|
26
|
//I2CDriver::osci2.write(1);
|
humlet |
14:352609d395c1
|
27
|
// read from MPU600's fifo
|
humlet |
14:352609d395c1
|
28
|
i2c.frequency(g_freq);
|
humlet |
14:352609d395c1
|
29
|
uint32_t t1=us_ticker_read();
|
humlet |
14:352609d395c1
|
30
|
//I2CDriver::osci2.write(0);
|
humlet |
14:352609d395c1
|
31
|
int stat = i2c.read(i2cAdr, reg, result, g_len);
|
humlet |
14:352609d395c1
|
32
|
uint32_t dt=us_ticker_read()-t1;
|
humlet |
14:352609d395c1
|
33
|
if(stat!=0) {
|
humlet |
14:352609d395c1
|
34
|
printf("\n%x %d %d %d\n",stat,g_freq,g_len,dt);
|
humlet |
14:352609d395c1
|
35
|
exit(0);
|
humlet |
14:352609d395c1
|
36
|
}
|
humlet |
14:352609d395c1
|
37
|
int16_t val=((static_cast<int16_t>(result[0])<<8)|static_cast<int16_t>(result[1]));
|
humlet |
7:04824382eafb
|
38
|
|
humlet |
14:352609d395c1
|
39
|
if(--g_disco>0)printf("val=%8d dt=%4dus\n",val,dt);
|
humlet |
7:04824382eafb
|
40
|
}
|
humlet |
7:04824382eafb
|
41
|
|
humlet |
7:04824382eafb
|
42
|
int doit()
|
humlet |
7:04824382eafb
|
43
|
{
|
humlet |
14:352609d395c1
|
44
|
config(i2c);
|
humlet |
14:352609d395c1
|
45
|
|
humlet |
7:04824382eafb
|
46
|
RtosTimer highPrioTicker(highPrioCallBck, osTimerPeriodic, (void *)0);
|
humlet |
7:04824382eafb
|
47
|
|
humlet |
14:352609d395c1
|
48
|
Thread::wait(500);
|
humlet |
7:04824382eafb
|
49
|
highPrioTicker.start(1);
|
humlet |
10:e3d6c92ff222
|
50
|
|
humlet |
7:04824382eafb
|
51
|
#if defined(TARGET_LPC1768)
|
humlet |
7:04824382eafb
|
52
|
const int nTest=7;
|
humlet |
7:04824382eafb
|
53
|
const int freq[nTest]= {1e5, 1e5, 1e5, 4e5, 4e5, 4e5, 4e5};
|
humlet |
14:352609d395c1
|
54
|
const int len[nTest]= {1, 4, 7, 1, 6, 12, 36};
|
humlet |
7:04824382eafb
|
55
|
#elif defined(TARGET_LPC11U24)
|
humlet |
7:04824382eafb
|
56
|
const int nTest=5;
|
humlet |
7:04824382eafb
|
57
|
const int freq[nTest]= {1e5, 1e5, 4e5, 4e5, 4e5 };
|
humlet |
13:530968937ccb
|
58
|
const int len[nTest]= {1, 6, 1, 6, 32};
|
humlet |
7:04824382eafb
|
59
|
#endif
|
humlet |
7:04824382eafb
|
60
|
for(int i=0; i<nTest; ++i) {
|
humlet |
7:04824382eafb
|
61
|
g_freq = freq[i];
|
humlet |
7:04824382eafb
|
62
|
g_len = len[i];
|
humlet |
7:04824382eafb
|
63
|
printf("f=%d l=%d\n",g_freq,g_len);
|
humlet |
7:04824382eafb
|
64
|
Thread::wait(500);
|
humlet |
14:352609d395c1
|
65
|
//highPrioTicker.start(1);
|
humlet |
7:04824382eafb
|
66
|
const uint32_t dt=1e6;
|
humlet |
7:04824382eafb
|
67
|
uint32_t tStart = us_ticker_read();
|
humlet |
7:04824382eafb
|
68
|
uint32_t tLast = tStart;
|
humlet |
7:04824382eafb
|
69
|
uint32_t tAct = tStart;
|
humlet |
7:04824382eafb
|
70
|
uint32_t tMe=0;
|
humlet |
14:352609d395c1
|
71
|
do { // loop an count consecutive µs ticker edges
|
humlet |
14:352609d395c1
|
72
|
//osci.write(!osci.read());
|
humlet |
7:04824382eafb
|
73
|
tAct=us_ticker_read();
|
humlet |
14:352609d395c1
|
74
|
#if defined(TARGET_LPC1768)
|
humlet |
14:352609d395c1
|
75
|
if(tAct==tLast+1)++tMe;
|
humlet |
14:352609d395c1
|
76
|
#elif defined(TARGET_LPC11U24)
|
humlet |
14:352609d395c1
|
77
|
uint32_t delta = tAct-tLast;
|
humlet |
14:352609d395c1
|
78
|
if(delta<=2)tMe+=delta; // on the 11U24 this loop takes a bit longer than 1µs (ISR ~3µs, task switch ~8µs)
|
humlet |
14:352609d395c1
|
79
|
#endif
|
humlet |
7:04824382eafb
|
80
|
tLast=tAct;
|
humlet |
7:04824382eafb
|
81
|
} while(tAct-tStart<dt);
|
humlet |
14:352609d395c1
|
82
|
//highPrioTicker.stop();
|
humlet |
14:352609d395c1
|
83
|
// and calculate the duty cycle from this measurement
|
humlet |
7:04824382eafb
|
84
|
printf("dc=%5.2f \n", 100.0*(float)tMe/dt);
|
humlet |
14:352609d395c1
|
85
|
g_disco=5;
|
humlet |
7:04824382eafb
|
86
|
while(g_disco>0);
|
humlet |
7:04824382eafb
|
87
|
}
|
humlet |
7:04824382eafb
|
88
|
return 0;
|
humlet |
7:04824382eafb
|
89
|
}
|
humlet |
7:04824382eafb
|
90
|
|
humlet |
7:04824382eafb
|
91
|
void readModWrite(I2CMasterRtos& i2c, uint8_t reg, uint8_t dta)
|
humlet |
10:e3d6c92ff222
|
92
|
{
|
humlet |
7:04824382eafb
|
93
|
char rd1;
|
humlet |
7:04824382eafb
|
94
|
int rStat1 = i2c.read(i2cAdr, reg, &rd1, 1);
|
humlet |
7:04824382eafb
|
95
|
char data[2];
|
humlet |
7:04824382eafb
|
96
|
data[0]=(char)reg;
|
humlet |
7:04824382eafb
|
97
|
data[1]=(char)dta;
|
humlet |
7:04824382eafb
|
98
|
char rd2;
|
humlet |
7:04824382eafb
|
99
|
int wStat = i2c.write(i2cAdr, data, 2);
|
humlet |
7:04824382eafb
|
100
|
osDelay(100);
|
humlet |
7:04824382eafb
|
101
|
int rStat2 = i2c.read(i2cAdr, reg, &rd2, 1);
|
humlet |
12:6ddadcbbdca2
|
102
|
printf("(%3x%3x%3x) %2x <- %2x => %2x -> %2x \n", rStat1, wStat, rStat2, reg, dta, rd1, rd2);
|
humlet |
7:04824382eafb
|
103
|
}
|
humlet |
7:04824382eafb
|
104
|
|
humlet |
7:04824382eafb
|
105
|
static void config(I2CMasterRtos& i2c)
|
humlet |
7:04824382eafb
|
106
|
{
|
humlet |
7:04824382eafb
|
107
|
uint8_t ncfg=32;
|
humlet |
7:04824382eafb
|
108
|
uint8_t regs[ncfg];
|
humlet |
7:04824382eafb
|
109
|
uint8_t vals[ncfg];
|
humlet |
7:04824382eafb
|
110
|
int cnt=0;
|
humlet |
7:04824382eafb
|
111
|
regs[cnt]=0x6b;
|
humlet |
7:04824382eafb
|
112
|
vals[cnt++]=(1<<7); // pwr 1 reg //: device reset
|
humlet |
7:04824382eafb
|
113
|
regs[cnt]=0x6b;
|
humlet |
7:04824382eafb
|
114
|
vals[cnt++]=1; // pwr 1 reg // clock from x gyro all pwr sav modes off
|
humlet |
7:04824382eafb
|
115
|
regs[cnt]=0x19;
|
humlet |
7:04824382eafb
|
116
|
vals[cnt++]=0; // sample rate divider reg // sapmle rate = gyro rate / (1+x)
|
humlet |
7:04824382eafb
|
117
|
regs[cnt]=0x1a;
|
humlet |
10:e3d6c92ff222
|
118
|
vals[cnt++]=0;// conf reg // no ext frame sync / no dig low pass set to 1 => 8kHz Sampling
|
humlet |
7:04824382eafb
|
119
|
regs[cnt]=0x1b;
|
humlet |
7:04824382eafb
|
120
|
vals[cnt++]=0;// gyro conf reg // no test mode and gyro range 250°/s
|
humlet |
7:04824382eafb
|
121
|
regs[cnt]=0x1c;
|
humlet |
7:04824382eafb
|
122
|
vals[cnt++]=0;// accl conf reg // no test mode and accl range 2g
|
humlet |
7:04824382eafb
|
123
|
regs[cnt]=0x23;
|
humlet |
7:04824382eafb
|
124
|
vals[cnt++]=0xf<<3;// fifo conf reg // accl + all gyro -> fifo
|
humlet |
7:04824382eafb
|
125
|
regs[cnt]=0x6a;
|
humlet |
7:04824382eafb
|
126
|
vals[cnt++]=(1<<2); // pwr 1 reg // fifo reset
|
humlet |
7:04824382eafb
|
127
|
regs[cnt]=0x6a;
|
humlet |
7:04824382eafb
|
128
|
vals[cnt++]=(1<<6); // pwr 1 reg // fifo on
|
humlet |
7:04824382eafb
|
129
|
|
humlet |
7:04824382eafb
|
130
|
for(int i=0; i<cnt; i++)
|
humlet |
7:04824382eafb
|
131
|
readModWrite(i2c, regs[i], vals[i]);
|
humlet |
7:04824382eafb
|
132
|
} |