1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134 | #define MPU6050_ACCEL_XOUT_H 0x3B // R
#define MPU6050_PWR_MGMT_1 0x6B // R/W
#define MPU6050_PWR_MGMT_2 0x6C // R/W
#define MPU6050_WHO_AM_I 0x75 // R
#define MPU6050_I2C_ADDRESS 0x68
....
typedef union accel_t_gyro_union
{
struct
{
uint8_t x_accel_h;
uint8_t x_accel_l;
uint8_t y_accel_h;
uint8_t y_accel_l;
uint8_t z_accel_h;
uint8_t z_accel_l;
uint8_t t_h;
uint8_t t_l;
uint8_t x_gyro_h;
uint8_t x_gyro_l;
uint8_t y_gyro_h;
uint8_t y_gyro_l;
uint8_t z_gyro_h;
uint8_t z_gyro_l;
}
reg;
struct
{
int x_accel;
int y_accel;
int z_accel;
int temperature;
int x_gyro;
int y_gyro;
int z_gyro;
}
value;
};
...
void setup(){
...
error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
Serial.print(F("WHO_AM_I : "));
Serial.print(c,HEX);
Serial.print(F(", error = "));
Serial.println(error,DEC);
error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1);
Serial.print(F("PWR_MGMT_2 : "));
Serial.print(c,HEX);
Serial.print(F(", error = "));
Serial.println(error,DEC);
MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
...
}
void loop(){
........
int error;
double dT;
accel_t_gyro_union accel_t_gyro;
Serial.println(F(""));
Serial.println(F("MPU-6050"));
// Read the raw values.
// Read 14 bytes at once,
// containing acceleration, temperature and gyro.
// With the default settings of the MPU-6050,
// there is no filter enabled, and the values
// are not very stable.
error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
Serial.print(F("Read accel, temp and gyro, error = "));
Serial.println(error,DEC);
// Swap all high and low bytes.
// After this, the registers values are swapped,
// so the structure name like x_accel_l does no
// longer contain the lower byte.
uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);
dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
...........
}
int MPU6050_read(int start, uint8_t *buffer, int size)
{
int i, n, error;
Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start);
if (n != 1)
return (-10);
n = Wire.endTransmission(false); // hold the I2C-bus
if (n != 0)
return (n);
// Third parameter is true: relase I2C-bus after data is read.
Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
i = 0;
while(Wire.available() && i<size)
{
buffer[i++]=Wire.read();
}
if ( i != size)
return (-11);
return (0); // return : no error
}
int MPU6050_write(int start, const uint8_t *pData, int size)
{
int n, error;
Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start); // write the start address
if (n != 1)
return (-20);
n = Wire.write(pData, size); // write data bytes
if (n != size)
return (-21);
error = Wire.endTransmission(true); // release the I2C-bus
if (error != 0)
return (error);
return (0); // return : no error
}
int MPU6050_write_reg(int reg, uint8_t data)
{
int error;
error = MPU6050_write(reg, &data, 1);
return (error);
}
|
