Matthias Grob / Mbed 2 deprecated FlyBed2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Dependents:   fluy343

IMU/Sensors/Gyro_Acc/MPU6050.cpp@8:e79c7939d6de, 2014-07-14 (annotated)

Committer:
maetugr
Date:
Mon Jul 14 09:06:43 2014 +0000
Revision:
8:e79c7939d6de
Parent:
7:ac2895479e34
Child:
10:14390c90c3f5
Perfect flight in acro mode and X-Config (JFYI oscillations were because of controller mount, had to screw the controller pretty stiff to the frame but with rubber washers from an old CD-Drive)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 4:b0a60b0b24a9 1 #include "MPU6050.h"
maetugr 4:b0a60b0b24a9 2
maetugr 4:b0a60b0b24a9 3 MPU6050::MPU6050(PinName sda, PinName scl) : I2C_Sensor(sda, scl, MPU6050_I2C_ADDRESS)
maetugr 4:b0a60b0b24a9 4 {
maetugr 4:b0a60b0b24a9 5 // Turns on the MPU6050's gyro and initializes it
maetugr 4:b0a60b0b24a9 6 // register datasheet: http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf
maetugr 4:b0a60b0b24a9 7 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)
maetugr 8:e79c7939d6de 8 writeRegister(MPU6050_RA_CONFIG, 0x03);
maetugr 8:e79c7939d6de 9 /*
maetugr 8:e79c7939d6de 10 last 3 Bits of |Accelerometer(Fs=1kHz) |Gyroscope
maetugr 8:e79c7939d6de 11 MPU6050_RA_CONFIG|Bandwidth(Hz)|Delay(ms)|Bandwidth(Hz)|Delay(ms)|Fs(kHz)
maetugr 8:e79c7939d6de 12 -------------------------------------------------------------------------
maetugr 8:e79c7939d6de 13 0 |260 |0 |256 |0.98 |8
maetugr 8:e79c7939d6de 14 1 |184 |2.0 |188 |1.9 |1
maetugr 8:e79c7939d6de 15 2 |94 |3.0 |98 |2.8 |1
maetugr 8:e79c7939d6de 16 3 |44 |4.9 |42 |4.8 |1
maetugr 8:e79c7939d6de 17 4 |21 |8.5 |20 |8.3 |1
maetugr 8:e79c7939d6de 18 5 |10 |13.8 |10 |13.4 |1
maetugr 8:e79c7939d6de 19 6 |5 |19.0 |5 |18.6 |1
maetugr 8:e79c7939d6de 20 */
maetugr 4:b0a60b0b24a9 21 writeRegister(MPU6050_RA_GYRO_CONFIG, 0x18); // scales gyros range to +-2000dps
maetugr 4:b0a60b0b24a9 22 writeRegister(MPU6050_RA_ACCEL_CONFIG, 0x00); // scales accelerometers range to +-2g
maetugr 4:b0a60b0b24a9 23 }
maetugr 4:b0a60b0b24a9 24
maetugr 4:b0a60b0b24a9 25 void MPU6050::read()
maetugr 4:b0a60b0b24a9 26 {
maetugr 4:b0a60b0b24a9 27 readraw_gyro(); // read raw measurement data
maetugr 4:b0a60b0b24a9 28 readraw_acc();
maetugr 4:b0a60b0b24a9 29
maetugr 4:b0a60b0b24a9 30 for (int i = 0; i < 3; i++)
maetugr 5:06e978fd147a 31 data_gyro[i] = (raw_gyro[i] - offset_gyro[i])*0.07; // subtract offset from calibration and multiply unit factor to get degree per second (datasheet s.10)
maetugr 4:b0a60b0b24a9 32
maetugr 4:b0a60b0b24a9 33 for (int i = 0; i < 3; i++)
maetugr 4:b0a60b0b24a9 34 data_acc[i] = raw_acc[i] - offset_acc[i]; // TODO: didn't care about units because IMU-algorithm just uses vector direction
maetugr 5:06e978fd147a 35
maetugr 5:06e978fd147a 36 // 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)
maetugr 5:06e978fd147a 37 float tmp = 0;
maetugr 5:06e978fd147a 38 tmp = data_gyro[0];
maetugr 5:06e978fd147a 39 data_gyro[0] = data_gyro[1];
maetugr 5:06e978fd147a 40 data_gyro[1] = -tmp;
maetugr 5:06e978fd147a 41 data_gyro[2] = data_gyro[2];
maetugr 5:06e978fd147a 42 tmp = data_acc[0];
maetugr 5:06e978fd147a 43 data_acc[0] = data_acc[1];
maetugr 5:06e978fd147a 44 data_acc[1] = -tmp;
maetugr 5:06e978fd147a 45 data_acc[2] = data_acc[2];
maetugr 4:b0a60b0b24a9 46 }
maetugr 4:b0a60b0b24a9 47
maetugr 4:b0a60b0b24a9 48 int MPU6050::readTemp()
maetugr 4:b0a60b0b24a9 49 {
maetugr 4:b0a60b0b24a9 50 char buffer[2]; // 8-Bit pieces of temperature data
maetugr 4:b0a60b0b24a9 51
maetugr 4:b0a60b0b24a9 52 readMultiRegister(MPU6050_RA_TEMP_OUT_H, buffer, 2); // read the sensors register for the temperature
maetugr 4:b0a60b0b24a9 53 return (short) (buffer[0] << 8 | buffer[1]);
maetugr 4:b0a60b0b24a9 54 }
maetugr 4:b0a60b0b24a9 55
maetugr 4:b0a60b0b24a9 56 void MPU6050::readraw_gyro()
maetugr 4:b0a60b0b24a9 57 {
maetugr 4:b0a60b0b24a9 58 char buffer[6]; // 8-Bit pieces of axis data
maetugr 4:b0a60b0b24a9 59
maetugr 5:06e978fd147a 60 if(readMultiRegister(MPU6050_RA_GYRO_XOUT_H | (1 << 7), buffer, 6) != 0) return; // read axis registers using I2C // TODO: why?! | (1 << 7)
maetugr 4:b0a60b0b24a9 61
maetugr 4:b0a60b0b24a9 62 raw_gyro[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
maetugr 4:b0a60b0b24a9 63 raw_gyro[1] = (short) (buffer[2] << 8 | buffer[3]);
maetugr 4:b0a60b0b24a9 64 raw_gyro[2] = (short) (buffer[4] << 8 | buffer[5]);
maetugr 4:b0a60b0b24a9 65 }
maetugr 4:b0a60b0b24a9 66
maetugr 4:b0a60b0b24a9 67 void MPU6050::readraw_acc()
maetugr 4:b0a60b0b24a9 68 {
maetugr 4:b0a60b0b24a9 69 char buffer[6]; // 8-Bit pieces of axis data
maetugr 4:b0a60b0b24a9 70
maetugr 4:b0a60b0b24a9 71 readMultiRegister(MPU6050_RA_ACCEL_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7)
maetugr 4:b0a60b0b24a9 72
maetugr 4:b0a60b0b24a9 73 raw_acc[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
maetugr 4:b0a60b0b24a9 74 raw_acc[1] = (short) (buffer[2] << 8 | buffer[3]);
maetugr 4:b0a60b0b24a9 75 raw_acc[2] = (short) (buffer[4] << 8 | buffer[5]);
maetugr 4:b0a60b0b24a9 76 }
maetugr 4:b0a60b0b24a9 77
maetugr 4:b0a60b0b24a9 78 void MPU6050::calibrate(int times, float separation_time)
maetugr 4:b0a60b0b24a9 79 {
maetugr 4:b0a60b0b24a9 80 // calibrate sensor with an average of count samples (result of calibration stored in offset[])
maetugr 4:b0a60b0b24a9 81 // Calibrate Gyroscope ----------------------------------
maetugr 4:b0a60b0b24a9 82 float calib_gyro[3] = {0,0,0}; // temporary array for the sum of calibration measurement
maetugr 4:b0a60b0b24a9 83
maetugr 4:b0a60b0b24a9 84 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
maetugr 4:b0a60b0b24a9 85 readraw_gyro();
maetugr 4:b0a60b0b24a9 86 for (int j = 0; j < 3; j++)
maetugr 4:b0a60b0b24a9 87 calib_gyro[j] += raw_gyro[j];
maetugr 4:b0a60b0b24a9 88 wait(separation_time);
maetugr 4:b0a60b0b24a9 89 }
maetugr 4:b0a60b0b24a9 90
maetugr 4:b0a60b0b24a9 91 for (int i = 0; i < 3; i++)
maetugr 4:b0a60b0b24a9 92 offset_gyro[i] = calib_gyro[i]/times; // take the average of the calibration measurements
maetugr 4:b0a60b0b24a9 93
maetugr 4:b0a60b0b24a9 94 // Calibrate Accelerometer -------------------------------
maetugr 4:b0a60b0b24a9 95 float calib_acc[3] = {0,0,0}; // temporary array for the sum of calibration measurement
maetugr 4:b0a60b0b24a9 96
maetugr 4:b0a60b0b24a9 97 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
maetugr 4:b0a60b0b24a9 98 readraw_acc();
maetugr 4:b0a60b0b24a9 99 for (int j = 0; j < 3; j++)
maetugr 4:b0a60b0b24a9 100 calib_acc[j] += raw_acc[j];
maetugr 4:b0a60b0b24a9 101 wait(separation_time);
maetugr 4:b0a60b0b24a9 102 }
maetugr 4:b0a60b0b24a9 103
maetugr 4:b0a60b0b24a9 104 for (int i = 0; i < 2; i++)
maetugr 4:b0a60b0b24a9 105 offset_acc[i] = calib_acc[i]/times; // take the average of the calibration measurements
maetugr 4:b0a60b0b24a9 106 }