1 /*
2 * Copyright (c) 2006-2018, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date Author Notes
8 * 2014-12-20 Bernard the first version
9 * 2015-1-11 RT_learning modify the mpu6050 initialize
10 */
11
12 #include <string.h>
13 #include <stdio.h>
14 #include <rtdevice.h>
15
16 #include "mpu6050_sensor.h"
17
18
19 const static sensor_t _MPU6050_sensor[] =
20 {
21 {
22 .name = "Accelerometer",
23 .vendor = "Invensense",
24 .version = sizeof(sensor_t),
25 .handle = 0,
26 .type = SENSOR_TYPE_ACCELEROMETER,
27 .maxRange = SENSOR_ACCEL_RANGE_16G,
28 .resolution = 1.0f,
29 .power = 0.5f,
30 .minDelay = 10000,
31 .fifoReservedEventCount = 0,
32 .fifoMaxEventCount = 64,
33 },
34 {
35 .name = "Gyroscope",
36 .vendor = "Invensense",
37 .version = sizeof(sensor_t),
38 .handle = 0,
39 .type = SENSOR_TYPE_GYROSCOPE,
40 .maxRange = SENSOR_GYRO_RANGE_2000DPS,
41 .resolution = 1.0f,
42 .power = 0.5f,
43 .minDelay = 10000,
44 .fifoReservedEventCount = 0,
45 .fifoMaxEventCount = 64,
46 }
47 };
48
MPU6050(int sensor_type,const char * iic_bus,int addr)49 MPU6050::MPU6050(int sensor_type, const char* iic_bus, int addr)
50 : SensorBase(sensor_type)
51 {
52 this->i2c_bus = (struct rt_i2c_bus_device *)rt_device_find(iic_bus);
53 if (this->i2c_bus == NULL)
54 {
55 printf("MPU6050: No IIC device:%s\n", iic_bus);
56 return;
57 }
58
59 this->i2c_addr = addr;
60
61 /* register to sensor manager */
62 SensorManager::registerSensor(this);
63 }
64
read_reg(rt_uint8_t reg,rt_uint8_t * value)65 int MPU6050::read_reg(rt_uint8_t reg, rt_uint8_t *value)
66 {
67 struct rt_i2c_msg msgs[2];
68
69 msgs[0].addr = this->i2c_addr;
70 msgs[0].flags = RT_I2C_WR;
71 msgs[0].buf = ®
72 msgs[0].len = 1;
73
74 msgs[1].addr = this->i2c_addr;
75 msgs[1].flags = RT_I2C_RD; /* Read from slave */
76 msgs[1].buf = (rt_uint8_t *)value;
77 msgs[1].len = 1;
78
79 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2)
80 return RT_EOK;
81
82 return -RT_ERROR;
83 }
84
read_buffer(rt_uint8_t reg,rt_uint8_t * value,rt_size_t size)85 int MPU6050::read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size)
86 {
87 struct rt_i2c_msg msgs[2];
88
89 msgs[0].addr = this->i2c_addr;
90 msgs[0].flags = RT_I2C_WR;
91 msgs[0].buf = ®
92 msgs[0].len = 1;
93
94 msgs[1].addr = this->i2c_addr;
95 msgs[1].flags = RT_I2C_RD; /* Read from slave */
96 msgs[1].buf = (rt_uint8_t *)value;
97 msgs[1].len = size;
98
99 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2)
100 return RT_EOK;
101
102 return -RT_ERROR;
103 }
104
write_reg(rt_uint8_t reg,rt_uint8_t value)105 int MPU6050::write_reg(rt_uint8_t reg, rt_uint8_t value)
106 {
107 struct rt_i2c_msg msgs[2];
108
109 msgs[0].addr = this->i2c_addr;
110 msgs[0].flags = RT_I2C_WR;
111 msgs[0].buf = ®
112 msgs[0].len = 1;
113
114 msgs[1].addr = this->i2c_addr;
115 msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START;
116 msgs[1].buf = (rt_uint8_t *)&value;
117 msgs[1].len = 1;
118
119 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2)
120 return RT_EOK;
121
122 return -RT_ERROR;
123 }
124
125
MPU6050_Accelerometer(const char * iic_name,int addr)126 MPU6050_Accelerometer::MPU6050_Accelerometer(const char* iic_name, int addr)
127 : MPU6050(SENSOR_TYPE_ACCELEROMETER, iic_name, addr)
128 {
129 int index;
130 uint8_t id;
131 rt_uint8_t value[6] = {0};
132 rt_int32_t x, y, z;
133 SensorConfig config = {SENSOR_MODE_NORMAL, SENSOR_DATARATE_400HZ, SENSOR_ACCEL_RANGE_2G};
134
135 /* initialize MPU6050 */
136 write_reg(MPU6050_PWR_MGMT_1, 0x80); /* reset mpu6050 device */
137 write_reg(MPU6050_SMPLRT_DIV, 0x00); /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */
138 write_reg(MPU6050_PWR_MGMT_1, 0x03); /* Wake up device , set device clock Z axis gyroscope */
139 write_reg(MPU6050_CONFIG, 0x03); /* set DLPF_CFG 42Hz */
140 write_reg(MPU6050_GYRO_CONFIG, 0x18); /* set gyro 2000deg/s */
141 write_reg(MPU6050_ACCEL_CONFIG, 0x08); /* set acc +-4g/s */
142
143
144 x_offset = y_offset = z_offset = 0;
145 x = y = z = 0;
146
147 /* read MPU6050 id */
148 read_buffer(MPU6050_WHOAMI, &id, 1);
149 if (id != MPU6050_ID)
150 {
151 printf("Warning: not found MPU6050 id: %02x\n", id);
152 }
153
154 /* get offset */
155 for (index = 0; index < 200; index ++)
156 {
157 read_buffer(MPU6050_ACCEL_XOUT_H, value, 6);
158
159 x += (((rt_int16_t)value[0] << 8) | value[1]);
160 y += (((rt_int16_t)value[2] << 8) | value[3]);
161 z += (((rt_int16_t)value[4] << 8) | value[5]);
162 }
163 x_offset = x / 200;
164 y_offset = y / 200;
165 z_offset = z / 200;
166
167 this->enable = RT_FALSE;
168 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G;
169 this->config = config;
170 }
171
172 int
configure(SensorConfig * config)173 MPU6050_Accelerometer::configure(SensorConfig *config)
174 {
175 int range;
176 uint8_t value;
177
178 if (config == RT_NULL) return -1;
179
180 /* TODO: set datarate */
181
182 /* get range and calc the sensitivity */
183 range = config->range.accel_range;
184 switch (range)
185 {
186 case SENSOR_ACCEL_RANGE_2G:
187 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G;
188 range = 0;
189 break;
190 case SENSOR_ACCEL_RANGE_4G:
191 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_4G;
192 range = 0x01 << 2;
193 break;
194 case SENSOR_ACCEL_RANGE_8G:
195 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_8G;
196 range = 0x02 << 2;
197 break;
198 case SENSOR_ACCEL_RANGE_16G:
199 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_16G;
200 range = 0x03 << 2;
201 break;
202 default:
203 return -1;
204 }
205
206 /* set range to sensor */
207 read_reg(MPU6050_ACCEL_CONFIG, &value);
208 value &= ~(0x3 << 2);
209 value |= range;
210 write_reg(MPU6050_ACCEL_CONFIG, value);
211
212 return 0;
213 }
214
215 int
activate(int enable)216 MPU6050_Accelerometer::activate(int enable)
217 {
218 uint8_t value;
219
220 if (enable && this->enable == RT_FALSE)
221 {
222 /* enable accelerometer */
223 read_reg(MPU6050_PWR_MGMT_2, &value);
224 value &= ~(0x07 << 2);
225 write_reg(MPU6050_PWR_MGMT_2, value);
226 }
227
228 if (!enable && this->enable == RT_TRUE)
229 {
230 /* disable accelerometer */
231 read_reg(MPU6050_PWR_MGMT_2, &value);
232 value |= (0x07 << 2);
233 write_reg(MPU6050_PWR_MGMT_2, value);
234 }
235
236 if (enable) this->enable = RT_TRUE;
237 else this->enable = RT_FALSE;
238
239 return 0;
240 }
241
242 int
poll(sensors_event_t * event)243 MPU6050_Accelerometer::poll(sensors_event_t *event)
244 {
245 rt_uint8_t value[6];
246 rt_int16_t x, y, z;
247
248 /* parameters check */
249 if (event == NULL) return -1;
250
251 /* get event data */
252 event->version = sizeof(sensors_event_t);
253 event->sensor = (int32_t) this;
254 event->timestamp = rt_tick_get();
255 event->type = SENSOR_TYPE_ACCELEROMETER;
256
257 read_buffer(MPU6050_ACCEL_XOUT_H, value, 6);
258
259 /* get raw data */
260 x = (((rt_int16_t)value[0] << 8) | value[1]);
261 y = (((rt_int16_t)value[2] << 8) | value[3]);
262 z = (((rt_int16_t)value[4] << 8) | value[5]);
263
264 if (config.mode == SENSOR_MODE_RAW)
265 {
266 event->raw_acceleration.x = x;
267 event->raw_acceleration.y = y;
268 event->raw_acceleration.z = z;
269 }
270 else
271 {
272
273 x -= x_offset; y -= y_offset; z -= z_offset;
274 event->acceleration.x = x * this->sensitivity * SENSORS_GRAVITY_STANDARD;
275 event->acceleration.y = y * this->sensitivity * SENSORS_GRAVITY_STANDARD;
276 event->acceleration.z = z * this->sensitivity * SENSORS_GRAVITY_STANDARD;
277 }
278
279 return 0;
280 }
281
282 void
getSensor(sensor_t * sensor)283 MPU6050_Accelerometer::getSensor(sensor_t *sensor)
284 {
285 /* get sensor description */
286 if (sensor)
287 {
288 memcpy(sensor, &_MPU6050_sensor[0], sizeof(sensor_t));
289 }
290 }
291
MPU6050_Gyroscope(const char * iic_name,int addr)292 MPU6050_Gyroscope::MPU6050_Gyroscope(const char* iic_name, int addr)
293 : MPU6050(SENSOR_TYPE_GYROSCOPE, iic_name, addr)
294 {
295 int index;
296 uint8_t id;
297 rt_uint8_t value[6];
298 rt_int32_t x, y, z;
299
300 /* initialize MPU6050 */
301 write_reg(MPU6050_PWR_MGMT_1, 0x80); /* reset mpu6050 device */
302 write_reg(MPU6050_SMPLRT_DIV, 0x00); /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */
303 write_reg(MPU6050_PWR_MGMT_1, 0x03); /* Wake up device , set device clock Z axis gyroscope */
304 write_reg(MPU6050_CONFIG, 0x03); /* set DLPF_CFG 42Hz */
305 write_reg(MPU6050_GYRO_CONFIG, 0x18); /* set gyro 2000deg/s */
306 write_reg(MPU6050_ACCEL_CONFIG, 0x08); /* set acc +-4g/s */
307
308 x_offset = y_offset = z_offset = 0;
309 x = y = z = 0;
310
311 /* read MPU6050 id */
312 read_reg(MPU6050_WHOAMI, &id);
313 if (id != MPU6050_ID)
314 {
315 printf("Warning: not found MPU6050 id: %02x\n", id);
316 }
317
318 /* get offset */
319 for (index = 0; index < 200; index ++)
320 {
321 read_buffer(MPU6050_GYRO_XOUT_H, value, 6);
322
323 x += (((rt_int16_t)value[0] << 8) | value[1]);
324 y += (((rt_int16_t)value[2] << 8) | value[3]);
325 z += (((rt_int16_t)value[4] << 8) | value[5]);
326 }
327 x_offset = x / 200;
328 y_offset = y / 200;
329 z_offset = z / 200;
330
331 this->enable = RT_FALSE;
332 this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS;
333 }
334
335 int
configure(SensorConfig * config)336 MPU6050_Gyroscope::configure(SensorConfig *config)
337 {
338 int range;
339 uint8_t value;
340
341 if (config == RT_NULL) return -1;
342
343 /* TODO: set datarate */
344
345 /* get range and calc the sensitivity */
346 range = config->range.gyro_range;
347 switch (range)
348 {
349 case SENSOR_GYRO_RANGE_250DPS:
350 this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS;
351 range = 0;
352 break;
353 case SENSOR_GYRO_RANGE_500DPS:
354 this->sensitivity = SENSOR_GYRO_SENSITIVITY_500DPS;
355 range = 0x01 << 2;
356 break;
357 case SENSOR_GYRO_RANGE_1000DPS:
358 this->sensitivity = SENSOR_GYRO_SENSITIVITY_1000DPS;
359 range = 0x02 << 2;
360 break;
361 case SENSOR_GYRO_RANGE_2000DPS:
362 this->sensitivity = SENSOR_GYRO_SENSITIVITY_2000DPS;
363 range = 0x03 << 2;
364 break;
365 default:
366 return -1;
367 }
368
369 /* set range to sensor */
370 read_reg(MPU6050_GYRO_CONFIG, &value);
371 value &= ~(0x3 << 2);
372 value |= range;
373 write_reg(MPU6050_GYRO_CONFIG, value);
374
375 return 0;
376 }
377
378 int
activate(int enable)379 MPU6050_Gyroscope::activate(int enable)
380 {
381 uint8_t value;
382
383 if (enable && this->enable == RT_FALSE)
384 {
385 /* enable gyroscope */
386 read_reg(MPU6050_PWR_MGMT_1, &value);
387 value &= ~(0x01 << 4);
388 write_reg(MPU6050_PWR_MGMT_1, value);
389
390 read_reg(MPU6050_PWR_MGMT_2, &value);
391 value &= ~(0x07 << 0);
392 write_reg(MPU6050_PWR_MGMT_2, value);
393 }
394
395 if (!enable && this->enable == RT_TRUE)
396 {
397 /* disable gyroscope */
398 read_reg(MPU6050_PWR_MGMT_2, &value);
399 value |= (0x07 << 0);
400 write_reg(MPU6050_PWR_MGMT_2, value);
401 }
402
403 if (enable) this->enable = RT_TRUE;
404 else this->enable = RT_FALSE;
405
406 return 0;
407 }
408
409 int
poll(sensors_event_t * event)410 MPU6050_Gyroscope::poll(sensors_event_t *event)
411 {
412 rt_uint8_t value[6];
413 rt_int16_t x, y, z;
414
415 /* parameters check */
416 if (event == NULL) return -1;
417
418 /* get event data */
419 event->version = sizeof(sensors_event_t);
420 event->sensor = (int32_t) this;
421 event->timestamp = rt_tick_get();
422 event->type = SENSOR_TYPE_GYROSCOPE;
423
424 read_buffer(MPU6050_GYRO_XOUT_H, value, 6);
425
426 /* get raw data */
427 x = (((rt_int16_t)value[0] << 8) | value[1]);
428 y = (((rt_int16_t)value[2] << 8) | value[3]);
429 z = (((rt_int16_t)value[4] << 8) | value[5]);
430
431
432 if (config.mode == SENSOR_MODE_RAW)
433 {
434 event->raw_gyro.x = x;
435 event->raw_gyro.y = y;
436 event->raw_gyro.z = z;
437 }
438 else
439 {
440 x -= x_offset; y -= y_offset; z -= z_offset;
441 event->gyro.x = x * this->sensitivity * SENSORS_DPS_TO_RADS;
442 event->gyro.y = y * this->sensitivity * SENSORS_DPS_TO_RADS;
443 event->gyro.z = z * this->sensitivity * SENSORS_DPS_TO_RADS;
444 }
445
446 return 0;
447 }
448
449 void
getSensor(sensor_t * sensor)450 MPU6050_Gyroscope::getSensor(sensor_t *sensor)
451 {
452 /* get sensor description */
453 if (sensor)
454 {
455 memcpy(sensor, &_MPU6050_sensor[1], sizeof(sensor_t));
456 }
457 }
458
459