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     /*
00009     last 3 Bits of   |Accelerometer(Fs=1kHz) |Gyroscope 
00010     MPU6050_RA_CONFIG|Bandwidth(Hz)|Delay(ms)|Bandwidth(Hz)|Delay(ms)|Fs(kHz)
00011     ------------------------------------------------------------------------- 
00012     0                |260          |0        |256          |0.98     |8 
00013     1                |184          |2.0      |188          |1.9      |1 
00014     2                |94           |3.0      |98           |2.8      |1 
00015     3                |44           |4.9      |42           |4.8      |1 
00016     4                |21           |8.5      |20           |8.3      |1 
00017     5                |10           |13.8     |10           |13.4     |1 
00018     6                |5            |19.0     |5            |18.6     |1 
00019     */
00020     writeRegister(MPU6050_RA_CONFIG, 0x03);
00021     writeRegister(MPU6050_RA_GYRO_CONFIG, 0x18);        // scales gyros range to +-2000dps
00022     writeRegister(MPU6050_RA_ACCEL_CONFIG, 0x00);       // scales accelerometers range to +-2g
00023 }
00024 
00025 void MPU6050::read()
00026 {
00027     readraw_gyro();                                          // read raw measurement data
00028     readraw_acc();
00029     
00030     offset_gyro[0] = -35;                // TODO: make better calibration
00031     offset_gyro[1] = 3;
00032     offset_gyro[2] = 2;
00033     
00034     for (int i = 0; i < 3; i++)
00035         data_gyro[i] = (raw_gyro[i] - offset_gyro[i]) * 0.07 * 0.87; // subtract offset from calibration and multiply unit factor to get degree per second (datasheet p.10)
00036     
00037     for (int i = 0; i < 3; i++)
00038         data_acc[i] = raw_acc[i] - offset_acc[i];           // TODO: didn't care about units because IMU-algorithm just uses vector direction
00039 
00040     // I have to swich coordinates on my board to match the ones of the other sensors (clear this part if you use the raw coordinates of the sensor)
00041     float tmp = 0;
00042     tmp = data_gyro[0];
00043     data_gyro[0] = -data_gyro[0];
00044     data_gyro[1] = -data_gyro[1];
00045     data_gyro[2] = data_gyro[2];
00046     tmp = data_acc[0];
00047     data_acc[0] = -data_acc[0];
00048     data_acc[1] = -data_acc[1];
00049     data_acc[2] = data_acc[2];
00050 }
00051 
00052 int MPU6050::readTemp()
00053 {
00054     char buffer[2];                                     // 8-Bit pieces of temperature data
00055     
00056     readMultiRegister(MPU6050_RA_TEMP_OUT_H, buffer, 2);     // read the sensors register for the temperature
00057     return (short) (buffer[0] << 8 | buffer[1]);
00058 }
00059 
00060 void MPU6050::readraw_gyro()
00061 {
00062     char buffer[6];                                     // 8-Bit pieces of axis data
00063     
00064     if(readMultiRegister(MPU6050_RA_GYRO_XOUT_H | (1 << 7), buffer, 6) != 0) return; // read axis registers using I2C   // TODO: why?!   | (1 << 7)
00065     
00066     raw_gyro[0] = (short) (buffer[0] << 8 | buffer[1]);     // join 8-Bit pieces to 16-bit short integers
00067     raw_gyro[1] = (short) (buffer[2] << 8 | buffer[3]);
00068     raw_gyro[2] = (short) (buffer[4] << 8 | buffer[5]);
00069 }
00070 
00071 void MPU6050::readraw_acc()
00072 {
00073     char buffer[6];                                     // 8-Bit pieces of axis data
00074     
00075     readMultiRegister(MPU6050_RA_ACCEL_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C   // TODO: why?!   | (1 << 7)
00076     
00077     raw_acc[0] = (short) (buffer[0] << 8 | buffer[1]);     // join 8-Bit pieces to 16-bit short integers
00078     raw_acc[1] = (short) (buffer[2] << 8 | buffer[3]);
00079     raw_acc[2] = (short) (buffer[4] << 8 | buffer[5]);
00080 }
00081 
00082 void MPU6050::calibrate(int times, float separation_time)
00083 {
00084     // calibrate sensor with an average of count samples (result of calibration stored in offset[])
00085     // Calibrate Gyroscope ----------------------------------
00086     float calib_gyro[3] = {0,0,0};                           // temporary array for the sum of calibration measurement
00087     
00088     for (int i = 0; i < times; i++) {                   // read 'times' times the data in a very short time
00089         readraw_gyro();
00090         for (int j = 0; j < 3; j++)
00091             calib_gyro[j] += raw_gyro[j];
00092         wait(separation_time);
00093     }
00094     
00095     for (int i = 0; i < 3; i++)
00096         offset_gyro[i] = calib_gyro[i]/times;                     // take the average of the calibration measurements
00097     
00098     // Calibrate Accelerometer ------------------------------- 
00099     float calib_acc[3] = {0,0,0};                           // temporary array for the sum of calibration measurement
00100     
00101     for (int i = 0; i < times; i++) {                   // read 'times' times the data in a very short time
00102         readraw_acc();
00103         for (int j = 0; j < 3; j++)
00104             calib_acc[j] += raw_acc[j];
00105         wait(separation_time);
00106     }
00107     
00108     for (int i = 0; i < 2; i++)
00109         offset_acc[i] = calib_acc[i]/times;                     // take the average of the calibration measurements
00110 }