-
Notifications
You must be signed in to change notification settings - Fork 1
/
MPU6050.c
208 lines (178 loc) · 5.39 KB
/
MPU6050.c
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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
#include "MPU6050.h"
#include "main.h"
#include "i2c.h"
Struct_MPU6050 MPU6050;
static float LSB_Sensitivity_ACC;
static float LSB_Sensitivity_GYRO;
void MPU6050_Writebyte(uint8_t reg_addr, uint8_t val)
{
HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, reg_addr, I2C_MEMADD_SIZE_8BIT, &val, 1, 1);
}
void MPU6050_Writebytes(uint8_t reg_addr, uint8_t len, uint8_t* data)
{
HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, reg_addr, I2C_MEMADD_SIZE_8BIT, data, len, 1);
}
void MPU6050_Readbyte(uint8_t reg_addr, uint8_t* data)
{
HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, reg_addr, I2C_MEMADD_SIZE_8BIT, data, 1, 1);
}
void MPU6050_Readbytes(uint8_t reg_addr, uint8_t len, uint8_t* data)
{
HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, reg_addr, I2C_MEMADD_SIZE_8BIT, data, len, 1);
}
void MPU6050_Initialization(void)
{
HAL_Delay(50);
uint8_t who_am_i = 0;
printf("Checking MPU6050...\n");
MPU6050_Readbyte(MPU6050_WHO_AM_I, &who_am_i);
if(who_am_i == 0x68)
{
printf("MPU6050 who_am_i = 0x%02x...OK\n", who_am_i);
}
else
{
printf("ERROR!\n");
printf("MPU6050 who_am_i : 0x%02x should be 0x68\n", who_am_i);
while(1)
{
printf("who am i error. Can not recognize mpu6050\n");
HAL_Delay(100);
}
}
//Reset the whole module before initialization
MPU6050_Writebyte(MPU6050_PWR_MGMT_1, 0x1<<7);
HAL_Delay(100);
//Power Management setting
/* Default is sleep mode
* necessary to wake up MPU6050*/
MPU6050_Writebyte(MPU6050_PWR_MGMT_1, 0x00);
HAL_Delay(50);
//Sample rate divider
/*Sample Rate = Gyroscope Output Rate / (1 + SMPRT_DIV) */
// MPU6050_Writebyte(MPU6050_SMPRT_DIV, 0x00); // ACC output rate is 1kHz, GYRO output rate is 8kHz
MPU6050_Writebyte(MPU6050_SMPRT_DIV, 39); // Sample Rate = 200Hz
HAL_Delay(50);
//FSYNC and DLPF setting
/*DLPF is set to 0*/
MPU6050_Writebyte(MPU6050_CONFIG, 0x00);
HAL_Delay(50);
//GYRO FULL SCALE setting
/*FS_SEL Full Scale Range
0 +-250 degree/s
1 +-500 degree/s
2 +-1000 degree/s
3 +-2000 degree/s */
uint8_t FS_SCALE_GYRO = 0x0;
MPU6050_Writebyte(MPU6050_GYRO_CONFIG, FS_SCALE_GYRO<<3);
HAL_Delay(50);
//ACCEL FULL SCALE setting
/*FS_SEL Full Scale Range
0 +-2g
1 +-4g
2 +-8g
3 +-16g */
uint8_t FS_SCALE_ACC = 0x0;
MPU6050_Writebyte(MPU6050_ACCEL_CONFIG, FS_SCALE_ACC<<3);
HAL_Delay(50);
MPU6050_Get_LSB_Sensitivity(FS_SCALE_GYRO, FS_SCALE_ACC);
printf("LSB_Sensitivity_GYRO: %f, LSB_Sensitivity_ACC: %f\n",LSB_Sensitivity_GYRO, LSB_Sensitivity_ACC);
//Interrupt PIN setting
uint8_t INT_LEVEL = 0x0; //0 - active high, 1 - active low
uint8_t LATCH_INT_EN = 0x0; //0 - INT 50us pulse, 1 - interrupt clear required
uint8_t INT_RD_CLEAR = 0x1; //0 - INT flag cleared by reading INT_STATUS, 1 - INT flag cleared by any read operation
MPU6050_Writebyte(MPU6050_INT_PIN_CFG, (INT_LEVEL<<7)|(LATCH_INT_EN<<5)|(INT_RD_CLEAR<<4)); //
HAL_Delay(50);
//Interrupt enable setting
uint8_t DATA_RDY_EN = 0x1; // 1 - enable, 0 - disable
MPU6050_Writebyte(MPU6050_INT_ENABLE, DATA_RDY_EN);
HAL_Delay(50);
printf("MPU6050 setting is finished\n");
}
/*Get Raw Data from sensor*/
void MPU6050_Get6AxisRawData(Struct_MPU6050* mpu6050)
{
uint8_t data[14];
MPU6050_Readbytes(MPU6050_ACCEL_XOUT_H, 14, data);
mpu6050->acc_x_raw = (data[0] << 8) | data[1];
mpu6050->acc_y_raw = (data[2] << 8) | data[3];
mpu6050->acc_z_raw = (data[4] << 8) | data[5];
mpu6050->temperature_raw = (data[6] << 8) | data[7];
mpu6050->gyro_x_raw = ((data[8] << 8) | data[9]);
mpu6050->gyro_y_raw = ((data[10] << 8) | data[11]);
mpu6050->gyro_z_raw = ((data[12] << 8) | data[13]);
}
void MPU6050_Get_LSB_Sensitivity(uint8_t FS_SCALE_GYRO, uint8_t FS_SCALE_ACC)
{
switch(FS_SCALE_GYRO)
{
case 0:
LSB_Sensitivity_GYRO = 131.f;
break;
case 1:
LSB_Sensitivity_GYRO = 65.5f;
break;
case 2:
LSB_Sensitivity_GYRO = 32.8f;
break;
case 3:
LSB_Sensitivity_GYRO = 16.4f;
break;
}
switch(FS_SCALE_ACC)
{
case 0:
LSB_Sensitivity_ACC = 16384.f;
break;
case 1:
LSB_Sensitivity_ACC = 8192.f;
break;
case 2:
LSB_Sensitivity_ACC = 4096.f;
break;
case 3:
LSB_Sensitivity_ACC = 2048.f;
break;
}
}
/*Convert Unit. acc_raw -> g, gyro_raw -> degree per second*/
void MPU6050_DataConvert(Struct_MPU6050* mpu6050)
{
//printf("LSB_Sensitivity_GYRO: %f, LSB_Sensitivity_ACC: %f\n",LSB_Sensitivity_GYRO,LSB_Sensitivity_ACC);
mpu6050->acc_x = mpu6050->acc_x_raw / LSB_Sensitivity_ACC;
mpu6050->acc_y = mpu6050->acc_y_raw / LSB_Sensitivity_ACC;
mpu6050->acc_z = mpu6050->acc_z_raw / LSB_Sensitivity_ACC;
mpu6050->temperature = (float)(mpu6050->temperature_raw)/340+36.53;
mpu6050->gyro_x = mpu6050->gyro_x_raw / LSB_Sensitivity_GYRO;
mpu6050->gyro_y = mpu6050->gyro_y_raw / LSB_Sensitivity_GYRO;
mpu6050->gyro_z = mpu6050->gyro_z_raw / LSB_Sensitivity_GYRO;
}
int MPU6050_DataReady(void)
{
//old school way
/*
static uint8_t INT_STATE_FLAG = 0;
static uint8_t DATA_RDY_INT_FLAG = 0;
static uint8_t INT_PIN = 0;
INT_PIN = LL_GPIO_IsInputPinSet(MPU6050_INT_PORT, MPU6050_INT_PIN);
if(INT_PIN == 1)
{
MPU6050_Readbyte(MPU6050_INT_STATUS, &INT_STATE_FLAG); //flag cleared automatically within the sensor
DATA_RDY_INT_FLAG = INT_STATE_FLAG & 0x01;
if(DATA_RDY_INT_FLAG == 1)
{
INT_STATE_FLAG = 0; //flag clearing
DATA_RDY_INT_FLAG = 0;
INT_PIN = 0;
return 1;
}
}
return 0;
*/
return HAL_GPIO_ReadPin(MPU6050_INT_PORT, MPU6050_INT_PIN);
}
void MPU6050_ProcessData(Struct_MPU6050* mpu6050)
{
MPU6050_Get6AxisRawData(mpu6050);
MPU6050_DataConvert(mpu6050);
}