GlobLib
HAL and API libraries for MCUs and hardware.
mpu6050.c
Go to the documentation of this file.
1 
17 #include "mpu6050.h"
18 
19 //This is not ideal
20 extern uint8_t I2C_start(uint8_t i2c_address);
21 extern uint8_t I2C_address(uint8_t i2c_address, uint8_t address, uint8_t operation);
22 extern uint8_t I2C_write(uint8_t i2c_address, uint8_t byte);
23 extern uint8_t I2C_read(uint8_t i2c_address);
24 extern uint8_t I2C_repeatRead(uint8_t i2c_address);
25 extern uint8_t I2C_stop(uint8_t i2c_address);
26 
27 //This is less ideal
28 #define I2C_WRITE 0
29 #define I2C_READ 1
30 
31 uint8_t MPU6050_setup(MPU6050 *target, uint8_t i2c_num, uint8_t address){
32 
33  target->bus = i2c_num;
34  target->slaveAddress = address;
35 
36  if(MPU6050_readByte(target,MPU6050_ADD_WAI) != address){
37 
38  return 1;
39  }
40 
41  //Start up the device
42  MPU6050_writeByte(target,MPU6050_ADD_PWR_MGMT_1,0x1); //0x68 = power register, 0x1 = xgyro clock source
43 
44  //Get the gyro sensitivity
45  target->gyroSen = MPU6050_getGyr(target);
46 
47  //Get the accel sensitivity
48  target->accelSen = MPU6050_getAcc(target);
49  return 0;
50 }
51 
52 void MPU6050_calc(MPU6050 *target, mpu_data what, float *container){
53 
54  int16_t tempRead;
55 
56  switch(what){
57 
58  case MPU_ACCX:
59 
60  tempRead = MPU6050_readHalf(target,MPU6050_ADD_ACCX);
61  *container = (target->accelSen * tempRead)/MPU_DATA_RANGE;
62  break;
63  case MPU_ACCY:
64 
65  tempRead = MPU6050_readHalf(target,MPU6050_ADD_ACCY);
66  *container = (target->accelSen * tempRead)/MPU_DATA_RANGE;
67  break;
68  case MPU_ACCZ:
69 
70  tempRead = MPU6050_readHalf(target,MPU6050_ADD_ACCZ);
71  *container = (target->accelSen * tempRead)/MPU_DATA_RANGE;
72  break;
73  case MPU_GYRX:
74 
75  tempRead = MPU6050_readHalf(target,MPU6050_ADD_GYRX);
76  *container = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
77  break;
78  case MPU_GYRY:
79 
80  tempRead = MPU6050_readHalf(target,MPU6050_ADD_GYRY);
81  *container = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
82  break;
83  case MPU_GYRZ:
84 
85  tempRead = MPU6050_readHalf(target,MPU6050_ADD_GYRZ);
86  *container = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
87  break;
88  case MPU_TEMP:
89 
90  //Page 30 Datasheet, temperature measurement formula
91  tempRead = MPU6050_readHalf(target,MPU6050_ADD_TEMP);
92  *container = (tempRead/340.0f) + 36.53f;
93  break;
94  case MPU_ACC:
95 
96  I2C_start(target->bus);
97  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
98  I2C_write(target->bus,MPU6050_ADD_ACCX);
99  I2C_start(target->bus);
100  I2C_address(target->bus,target->slaveAddress,I2C_READ);
101 
102  //X-axis accel
103  tempRead = (I2C_repeatRead(target->bus) << 8);
104  tempRead |= (I2C_repeatRead(target->bus));
105  *container++ = (target->accelSen * tempRead)/MPU_DATA_RANGE;
106 
107  //Y-axis accel
108  tempRead = (I2C_repeatRead(target->bus) << 8);
109  tempRead |= (I2C_repeatRead(target->bus));
110  *container++ = (target->accelSen * tempRead)/MPU_DATA_RANGE;
111 
112  //Z-axis accel
113  tempRead = (I2C_repeatRead(target->bus) << 8);
114  I2C_stop(target->bus);
115  tempRead |= (I2C_read(target->bus));
116  *container = (target->accelSen * tempRead)/MPU_DATA_RANGE;
117 
118  break;
119  case MPU_GYR:
120 
121  I2C_start(target->bus);
122  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
123  I2C_write(target->bus,MPU6050_ADD_GYRX);
124  I2C_start(target->bus);
125  I2C_address(target->bus,target->slaveAddress,I2C_READ);
126 
127  //X-axis gyro
128  tempRead = (I2C_repeatRead(target->bus) << 8);
129  tempRead |= (I2C_repeatRead(target->bus));
130  *container++ = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
131 
132  //Y-axis gyro
133  tempRead = (I2C_repeatRead(target->bus) << 8);
134  tempRead |= (I2C_repeatRead(target->bus));
135  *container++ = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
136 
137  //Z-axis gyro
138  tempRead = (I2C_repeatRead(target->bus) << 8);
139  I2C_stop(target->bus);
140  tempRead |= (I2C_read(target->bus));
141  *container = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
142  break;
143 
144  case MPU_ALL:
145 
146  I2C_start(target->bus);
147  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
148  I2C_write(target->bus,MPU6050_ADD_ACCX);
149  I2C_start(target->bus);
150  I2C_address(target->bus,target->slaveAddress,I2C_READ);
151 
152  //X-axis accel
153  tempRead = (I2C_repeatRead(target->bus) << 8);
154  tempRead |= (I2C_repeatRead(target->bus));
155  *container++ = (target->accelSen * tempRead)/MPU_DATA_RANGE;
156 
157  //Y-axis accel
158  tempRead = (I2C_repeatRead(target->bus) << 8);
159  tempRead |= (I2C_repeatRead(target->bus));
160  *container++ = (target->accelSen * tempRead)/MPU_DATA_RANGE;
161 
162  //Z-axis accel
163  tempRead = (I2C_repeatRead(target->bus) << 8);
164  tempRead |= (I2C_repeatRead(target->bus));
165  *container++ = (target->accelSen * tempRead)/MPU_DATA_RANGE;
166 
167  //Temperature
168  tempRead = (I2C_repeatRead(target->bus) << 8);
169  tempRead |= (I2C_repeatRead(target->bus));
170  *container++ = (tempRead/340.0f) + 36.53f;
171 
172  //X-axis gyro
173  tempRead = (I2C_repeatRead(target->bus) << 8);
174  tempRead |= (I2C_repeatRead(target->bus));
175  *container++ = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
176 
177  //Y-axis gyro
178  tempRead = (I2C_repeatRead(target->bus) << 8);
179  tempRead |= (I2C_repeatRead(target->bus));
180  *container++ = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
181 
182  //Z-axis gyro
183  tempRead = (I2C_repeatRead(target->bus) << 8);
184  I2C_stop(target->bus);
185  tempRead |= (I2C_read(target->bus));
186  *container = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
187  break;
188 
189  case MPU_IMU:
190 
191  I2C_start(target->bus);
192  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
193  I2C_write(target->bus,MPU6050_ADD_ACCX);
194  I2C_start(target->bus);
195  I2C_address(target->bus,target->slaveAddress,I2C_READ);
196 
197  //X-axis accel
198  tempRead = (I2C_repeatRead(target->bus) << 8);
199  tempRead |= (I2C_repeatRead(target->bus));
200  *container++ = (target->accelSen * tempRead)/MPU_DATA_RANGE;
201 
202  //Y-axis accel
203  tempRead = (I2C_repeatRead(target->bus) << 8);
204  tempRead |= (I2C_repeatRead(target->bus));
205  *container++ = (target->accelSen * tempRead)/MPU_DATA_RANGE;
206 
207  //Z-axis accel
208  tempRead = (I2C_repeatRead(target->bus) << 8);
209  tempRead |= (I2C_repeatRead(target->bus));
210  *container++ = (target->accelSen * tempRead)/MPU_DATA_RANGE;
211 
212  //These two need to be read but not used, don't add to container
213  I2C_repeatRead(target->bus);
214  I2C_repeatRead(target->bus);
215 
216  //X-axis gyro
217  tempRead = (I2C_repeatRead(target->bus) << 8);
218  tempRead |= (I2C_repeatRead(target->bus));
219  *container++ = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
220 
221  //Y-axis gyro
222  tempRead = (I2C_repeatRead(target->bus) << 8);
223  tempRead |= (I2C_repeatRead(target->bus));
224  *container++ = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
225 
226  //Z-axis gyro
227  tempRead = (I2C_repeatRead(target->bus) << 8);
228  I2C_stop(target->bus);
229  tempRead |= (I2C_read(target->bus));
230  *container = (target->gyroSen * tempRead)/MPU_DATA_RANGE;
231  break;
232 
233  default:
234  break;
235  }
236  return;
237 }
238 void MPU6050_raw(MPU6050 *target, mpu_data what, int16_t *container){
239 
240  switch(what){
241 
242  case MPU_ACCX:
243 
244  *container = MPU6050_readHalf(target,MPU6050_ADD_ACCX);
245  break;
246  case MPU_ACCY:
247 
248  *container = MPU6050_readHalf(target,MPU6050_ADD_ACCY);
249  break;
250  case MPU_ACCZ:
251 
252  *container = MPU6050_readHalf(target,MPU6050_ADD_ACCZ);
253  break;
254  case MPU_GYRX:
255 
256  *container = MPU6050_readHalf(target,MPU6050_ADD_GYRX);
257  break;
258  case MPU_GYRY:
259 
260  *container = MPU6050_readHalf(target,MPU6050_ADD_GYRY);
261  break;
262  case MPU_GYRZ:
263 
264  *container = MPU6050_readHalf(target,MPU6050_ADD_GYRZ);
265  break;
266  case MPU_TEMP:
267 
268  *container = MPU6050_readHalf(target,MPU6050_ADD_TEMP);
269  break;
270  case MPU_ACC:
271 
272  I2C_start(target->bus);
273  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
274  I2C_write(target->bus,MPU6050_ADD_ACCX);
275  I2C_start(target->bus);
276  I2C_address(target->bus,target->slaveAddress,I2C_READ);
277  *container = (I2C_repeatRead(target->bus) << 8);
278  *container++ |= (I2C_repeatRead(target->bus));
279  *container = (I2C_repeatRead(target->bus) << 8);
280  *container++ |= (I2C_repeatRead(target->bus));
281  *container = (I2C_repeatRead(target->bus) << 8);
282  I2C_stop(target->bus);
283  *container++ |= (I2C_read(target->bus));
284  break;
285  case MPU_GYR:
286 
287  I2C_start(target->bus);
288  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
289  I2C_write(target->bus,MPU6050_ADD_GYRX);
290  I2C_start(target->bus);
291  I2C_address(target->bus,target->slaveAddress,I2C_READ);
292  *container = (I2C_repeatRead(target->bus) << 8);
293  *container++ |= (I2C_repeatRead(target->bus));
294  *container = (I2C_repeatRead(target->bus) << 8);
295  *container++ |= (I2C_repeatRead(target->bus));
296  *container = (I2C_repeatRead(target->bus) << 8);
297  I2C_stop(target->bus);
298  *container++ |= (I2C_read(target->bus));
299 
300  break;
301  case MPU_ALL:
302 
303  I2C_start(target->bus);
304  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
305  I2C_write(target->bus,MPU6050_ADD_ACCX);
306  I2C_start(target->bus);
307  I2C_address(target->bus,target->slaveAddress,I2C_READ);
308  *container = (I2C_repeatRead(target->bus) << 8);
309  *container++ |= (I2C_repeatRead(target->bus));
310  *container = (I2C_repeatRead(target->bus) << 8);
311  *container++ |= (I2C_repeatRead(target->bus));
312  *container = (I2C_repeatRead(target->bus) << 8);
313  *container++ |= (I2C_repeatRead(target->bus));
314  *container = (I2C_repeatRead(target->bus) << 8);
315  *container++ |= (I2C_repeatRead(target->bus));
316  *container = (I2C_repeatRead(target->bus) << 8);
317  *container++ |= (I2C_repeatRead(target->bus));
318  *container = (I2C_repeatRead(target->bus) << 8);
319  *container++ |= (I2C_repeatRead(target->bus));
320  *container = (I2C_repeatRead(target->bus) << 8);
321  I2C_stop(target->bus);
322  *container++ |= (I2C_read(target->bus));
323  break;
324 
325  case MPU_IMU:
326 
327  I2C_start(target->bus);
328  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
329  I2C_write(target->bus,MPU6050_ADD_ACCX);
330  I2C_start(target->bus);
331  I2C_address(target->bus,target->slaveAddress,I2C_READ);
332  *container = (I2C_repeatRead(target->bus) << 8);
333  *container++ |= (I2C_repeatRead(target->bus));
334  *container = (I2C_repeatRead(target->bus) << 8);
335  *container++ |= (I2C_repeatRead(target->bus));
336  *container = (I2C_repeatRead(target->bus) << 8);
337  *container++ |= (I2C_repeatRead(target->bus));
338 
339  //These two need to be read but not used, simply don't increment
340  I2C_repeatRead(target->bus);
341  I2C_repeatRead(target->bus);
342 
343  *container = (I2C_repeatRead(target->bus) << 8);
344  *container++ |= (I2C_repeatRead(target->bus));
345  *container = (I2C_repeatRead(target->bus) << 8);
346  *container++ |= (I2C_repeatRead(target->bus));
347  *container = (I2C_repeatRead(target->bus) << 8);
348  I2C_stop(target->bus);
349  *container++ |= (I2C_read(target->bus));
350  break;
351 
352  default:
353  break;
354  }
355  return;
356 }
357 
358 void MPU6050_writeByte(MPU6050 *target, uint8_t address, uint8_t data){
359 
360  I2C_start(target->bus);
361  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
362  I2C_write(target->bus,address);
363  I2C_write(target->bus,data);
364  I2C_stop(target->bus);
365  return;
366 }
367 
368 uint8_t MPU6050_readByte(MPU6050 *target, uint8_t address){
369 
370  I2C_start(target->bus);
371  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
372  I2C_write(target->bus,address);
373  I2C_start(target->bus);
374  I2C_address(target->bus,target->slaveAddress,I2C_READ);
375  I2C_stop(target->bus);
376  return I2C_read(target->bus);
377 
378 }
379 
380 uint16_t MPU6050_readHalf(MPU6050 *target, uint8_t address){
381 
382  uint16_t data;
383 
384  I2C_start(target->bus);
385  I2C_address(target->bus,target->slaveAddress,I2C_WRITE);
386  I2C_write(target->bus,address);
387  I2C_start(target->bus);
388  I2C_address(target->bus,target->slaveAddress,I2C_READ);
389  data = (I2C_repeatRead(target->bus) << 8);
390  I2C_stop(target->bus);
391  return (data | I2C_read(target->bus));
392 }
393 
394 void MPU6050_setAcc(MPU6050 *target, mpu_acc range){
395 
396  MPU6050_writeByte(target,MPU6050_ADD_ACCEL_CONFIG,range);
397 
398  switch (range){
399 
400  case MPU_ACC_2G:
401 
402  target->accelSen = 2;
403  break;
404  case MPU_ACC_4G:
405 
406  target->accelSen = 4;
407  break;
408  case MPU_ACC_8G:
409 
410  target->accelSen = 8;
411  break;
412  case MPU_ACC_16G:
413 
414  target->accelSen = 16;
415  break;
416  default:
417 
418  target->accelSen = 2;
419  break;
420  }
421  return;
422 
423 }
424 
425 void MPU6050_setGyr(MPU6050 *target, mpu_gyr range){
426 
427  MPU6050_writeByte(target,MPU6050_ADD_GYRO_CONFIG,range);
428 
429  switch (range){
430 
431  case MPU_GYR_250:
432 
433  target->gyroSen = 250;
434  break;
435  case MPU_GYR_500:
436 
437  target->gyroSen = 500;
438  break;
439  case MPU_GYR_1000:
440 
441  target->gyroSen = 1000;
442  break;
443  case MPU_GYR_2000:
444 
445  target->gyroSen = 2000;
446  break;
447  default:
448 
449  target->gyroSen = 250;
450  break;
451  }
452  return;
453 }
454 
455 uint8_t MPU6050_getAcc(MPU6050 *target){
456 
457  uint8_t data = MPU6050_readByte(target,MPU6050_ADD_ACCEL_CONFIG);
458 
459  switch (data){
460 
461  case MPU_ACC_2G:
462 
463  return 2;
464  break;
465  case MPU_ACC_4G:
466 
467  return 4;
468  break;
469  case MPU_ACC_8G:
470 
471  return 8;
472  break;
473  case MPU_ACC_16G:
474 
475  return 16;
476  break;
477  default:
478 
479  return 2;
480  break;
481  }
482  return 2;
483 }
484 
485 uint16_t MPU6050_getGyr(MPU6050 *target){
486 
487  uint8_t data = MPU6050_readByte(target,MPU6050_ADD_GYRO_CONFIG);
488 
489  switch (data){
490 
491  case MPU_GYR_250:
492 
493  return 250;
494  break;
495  case MPU_GYR_500:
496 
497  return 500;
498  break;
499  case MPU_GYR_1000:
500 
501  return 1000;
502  break;
503  case MPU_GYR_2000:
504 
505  return 2000;
506  break;
507  default:
508 
509  return 250;
510  break;
511  }
512  return 250;
513 }
MPU6050::slaveAddress
uint8_t slaveAddress
The address of the slave device.
Definition: mpu6050.h:72
MPU6050_getAcc
uint8_t MPU6050_getAcc(MPU6050 *target)
Get the acceleration range.
Definition: mpu6050.c:455
MPU_IMU
All gyro and all accelermeter.
Definition: mpu6050.h:97
MPU_GYR_1000
1000 degree range for gyroscope
Definition: mpu6050.h:115
MPU6050_writeByte
void MPU6050_writeByte(MPU6050 *target, uint8_t address, uint8_t data)
Writes a single byte to a given address.
Definition: mpu6050.c:358
MPU_GYRZ
Gyrometer data z-axis.
Definition: mpu6050.h:94
MPU_GYR_500
500 degree range for gyroscope
Definition: mpu6050.h:114
MPU_GYR_250
250 degree range for gyroscope
Definition: mpu6050.h:113
MPU6050_readByte
uint8_t MPU6050_readByte(MPU6050 *target, uint8_t address)
Read a single byte from a given address.
Definition: mpu6050.c:368
MPU_ACC_8G
8G range for accelerometer
Definition: mpu6050.h:106
MPU6050_setAcc
void MPU6050_setAcc(MPU6050 *target, mpu_acc range)
Set acceleration range.
Definition: mpu6050.c:394
MPU_ACC_4G
4G range for accelerometer
Definition: mpu6050.h:105
MPU6050::gyroSen
uint16_t gyroSen
The gyroscope sesitivity, set during MPU6050_setup() by reading slave data register.
Definition: mpu6050.h:80
MPU6050_setGyr
void MPU6050_setGyr(MPU6050 *target, mpu_gyr range)
Set gyroscope range.
Definition: mpu6050.c:425
MPU_ACC_16G
16G range for accelerometer
Definition: mpu6050.h:107
MPU_GYRY
Gyrometer data y-axis.
Definition: mpu6050.h:93
MPU6050_readHalf
uint16_t MPU6050_readHalf(MPU6050 *target, uint8_t address)
Read a half word from a given address.
Definition: mpu6050.c:380
MPU6050::accelSen
uint8_t accelSen
The accelerometer sesitivity, set during MPU6050_setup() by reading slave data register.
Definition: mpu6050.h:76
MPU_ACCX
Acceleration data x-axis.
Definition: mpu6050.h:88
MPU_GYRX
Gyrometer data x-axis.
Definition: mpu6050.h:92
MPU6050::bus
uint8_t bus
The port the device is connected to.
Definition: mpu6050.h:68
MPU_ALL
All raw data.
Definition: mpu6050.h:98
MPU_ACCY
Acceleration data y-axis.
Definition: mpu6050.h:89
MPU6050_raw
void MPU6050_raw(MPU6050 *target, mpu_data what, int16_t *container)
Get raw data values (16-bit each)
Definition: mpu6050.c:238
MPU_DATA_RANGE
#define MPU_DATA_RANGE
The decimal rangle of the ADC (15-bit <=> 32767).
Definition: mpu6050.h:58
MPU_ACC
All raw accelerometer data.
Definition: mpu6050.h:95
mpu_acc
mpu_acc
Definition: mpu6050.h:102
MPU6050_calc
void MPU6050_calc(MPU6050 *target, mpu_data what, float *container)
Get calculated data values.
Definition: mpu6050.c:52
mpu_gyr
mpu_gyr
Definition: mpu6050.h:111
mpu6050.h
Header file for Invensence IMU MPU6050.
MPU6050_setup
uint8_t MPU6050_setup(MPU6050 *target, uint8_t i2c_num, uint8_t address)
Initalizes the slave device and populates data structure variable.
Definition: mpu6050.c:31
MPU_ACCZ
Acceleration data z-axis.
Definition: mpu6050.h:90
mpu_data
mpu_data
Different options for getting data with MPU6050_raw() and MPU6050_calc()
Definition: mpu6050.h:86
MPU_TEMP
Temperature data.
Definition: mpu6050.h:91
MPU_GYR
All raw gyro data.
Definition: mpu6050.h:96
MPU_ACC_2G
2G range for accelerometer
Definition: mpu6050.h:104
MPU6050
Data structure containing variables specific to individual slave devices.
Definition: mpu6050.h:63
MPU_GYR_2000
2000 degree range for gyroscope
Definition: mpu6050.h:116
MPU6050_getGyr
uint16_t MPU6050_getGyr(MPU6050 *target)
Get the gyroscope range.
Definition: mpu6050.c:485