MPU6050.cpp

00001 #include "MPU6050.h"
00002 
00003 MPU6050::MPU6050(PinName sda, PinName scl) : I2C_Sensor(sda, scl, MPU6050_I2C_ADDRESS)
00004 {
00005     // Turns on the MPU6050's gyro and initializes it
00006     // register datasheet: http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf
00007     writeRegister(MPU6050_RA_PWR_MGMT_1, 0x01);         // wake up from sleep and chooses Gyro X-Axis as Clock source (stadard sleeping and with inacurate clock is 0x40)
00008     writeRegister(MPU6050_RA_GYRO_CONFIG, 0x18);        // scales gyros range to +-2000dps
00009     writeRegister(MPU6050_RA_ACCEL_CONFIG, 0x00);       // scales accelerometers range to +-2g
00010 }
00011 
00012 void MPU6050::read()
00013 {
00014     readraw_gyro();                                          // read raw measurement data
00015     readraw_acc();
00016     
00017     for (int i = 0; i < 3; i++)
00018         data_gyro[i] = (raw_gyro[i] - offset_gyro[i])*0.07;             // subtract offset from calibration and multiply unit factor (datasheet s.10)
00019     
00020     for (int i = 0; i < 3; i++)
00021         data_acc[i] = raw_acc[i] - offset_acc[i];             // TODO: didn't care about units because IMU-algorithm just uses vector direction
00022 }
00023 
00024 int MPU6050::readTemp()
00025 {
00026     char buffer[2];                                     // 8-Bit pieces of temperature data
00027     
00028     readMultiRegister(MPU6050_RA_TEMP_OUT_H, buffer, 2);     // read the sensors register for the temperature
00029     return (short) (buffer[0] << 8 | buffer[1]);
00030 }
00031 
00032 void MPU6050::readraw_gyro()
00033 {
00034     char buffer[6];                                     // 8-Bit pieces of axis data
00035     
00036     readMultiRegister(MPU6050_RA_GYRO_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C   // TODO: why?!   | (1 << 7)
00037     
00038     raw_gyro[0] = (short) (buffer[0] << 8 | buffer[1]);     // join 8-Bit pieces to 16-bit short integers
00039     raw_gyro[1] = (short) (buffer[2] << 8 | buffer[3]);
00040     raw_gyro[2] = (short) (buffer[4] << 8 | buffer[5]);
00041 }
00042 
00043 void MPU6050::readraw_acc()
00044 {
00045     char buffer[6];                                     // 8-Bit pieces of axis data
00046     
00047     readMultiRegister(MPU6050_RA_ACCEL_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C   // TODO: why?!   | (1 << 7)
00048     
00049     raw_acc[0] = (short) (buffer[0] << 8 | buffer[1]);     // join 8-Bit pieces to 16-bit short integers
00050     raw_acc[1] = (short) (buffer[2] << 8 | buffer[3]);
00051     raw_acc[2] = (short) (buffer[4] << 8 | buffer[5]);
00052 }
00053 
00054 void MPU6050::calibrate(int times, float separation_time)
00055 {
00056     // calibrate sensor with an average of count samples (result of calibration stored in offset[])
00057     // Calibrate Gyroscope ----------------------------------
00058     float calib_gyro[3] = {0,0,0};                           // temporary array for the sum of calibration measurement
00059     
00060     for (int i = 0; i < times; i++) {                   // read 'times' times the data in a very short time
00061         readraw_gyro();
00062         for (int j = 0; j < 3; j++)
00063             calib_gyro[j] += raw_gyro[j];
00064         wait(separation_time);
00065     }
00066     
00067     for (int i = 0; i < 3; i++)
00068         offset_gyro[i] = calib_gyro[i]/times;                     // take the average of the calibration measurements
00069     
00070     // Calibrate Accelerometer ------------------------------- 
00071     float calib_acc[3] = {0,0,0};                           // temporary array for the sum of calibration measurement
00072     
00073     for (int i = 0; i < times; i++) {                   // read 'times' times the data in a very short time
00074         readraw_acc();
00075         for (int j = 0; j < 3; j++)
00076             calib_acc[j] += raw_acc[j];
00077         wait(separation_time);
00078     }
00079     
00080     for (int i = 0; i < 2; i++)
00081         offset_acc[i] = calib_acc[i]/times;                     // take the average of the calibration measurements
00082 }