Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

Sensors/Gyro_Acc/MPU6050.cpp@40:2ca410923691, 2014-02-14 (annotated)

Committer:
maetugr
Date:
Fri Feb 14 14:17:32 2014 +0000
Revision:
40:2ca410923691
now with MPU6050 before taking it too FlyBed2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 40:2ca410923691 1 #include "MPU6050.h"
maetugr 40:2ca410923691 2
maetugr 40:2ca410923691 3 MPU6050::MPU6050(PinName sda, PinName scl) : I2C_Sensor(sda, scl, MPU6050_I2C_ADDRESS)
maetugr 40:2ca410923691 4 {
maetugr 40:2ca410923691 5 // Turns on the MPU6050's gyro and initializes it
maetugr 40:2ca410923691 6 // register datasheet: http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf
maetugr 40:2ca410923691 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 40:2ca410923691 8 writeRegister(MPU6050_RA_GYRO_CONFIG, 0x18); // scales gyros range to +-2000dps
maetugr 40:2ca410923691 9 writeRegister(MPU6050_RA_ACCEL_CONFIG, 0x00); // scales accelerometers range to +-2g
maetugr 40:2ca410923691 10 }
maetugr 40:2ca410923691 11
maetugr 40:2ca410923691 12 void MPU6050::read()
maetugr 40:2ca410923691 13 {
maetugr 40:2ca410923691 14 readraw_gyro(); // read raw measurement data
maetugr 40:2ca410923691 15 readraw_acc();
maetugr 40:2ca410923691 16
maetugr 40:2ca410923691 17 for (int i = 0; i < 3; i++)
maetugr 40:2ca410923691 18 data_gyro[i] = (raw_gyro[i] - offset_gyro[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10)
maetugr 40:2ca410923691 19
maetugr 40:2ca410923691 20 for (int i = 0; i < 3; i++)
maetugr 40:2ca410923691 21 data_acc[i] = raw_acc[i] - offset_acc[i]; // TODO: didn't care about units because IMU-algorithm just uses vector direction
maetugr 40:2ca410923691 22 }
maetugr 40:2ca410923691 23
maetugr 40:2ca410923691 24 int MPU6050::readTemp()
maetugr 40:2ca410923691 25 {
maetugr 40:2ca410923691 26 char buffer[2]; // 8-Bit pieces of temperature data
maetugr 40:2ca410923691 27
maetugr 40:2ca410923691 28 readMultiRegister(MPU6050_RA_TEMP_OUT_H, buffer, 2); // read the sensors register for the temperature
maetugr 40:2ca410923691 29 return (short) (buffer[0] << 8 | buffer[1]);
maetugr 40:2ca410923691 30 }
maetugr 40:2ca410923691 31
maetugr 40:2ca410923691 32 void MPU6050::readraw_gyro()
maetugr 40:2ca410923691 33 {
maetugr 40:2ca410923691 34 char buffer[6]; // 8-Bit pieces of axis data
maetugr 40:2ca410923691 35
maetugr 40:2ca410923691 36 readMultiRegister(MPU6050_RA_GYRO_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7)
maetugr 40:2ca410923691 37
maetugr 40:2ca410923691 38 raw_gyro[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
maetugr 40:2ca410923691 39 raw_gyro[1] = (short) (buffer[2] << 8 | buffer[3]);
maetugr 40:2ca410923691 40 raw_gyro[2] = (short) (buffer[4] << 8 | buffer[5]);
maetugr 40:2ca410923691 41 }
maetugr 40:2ca410923691 42
maetugr 40:2ca410923691 43 void MPU6050::readraw_acc()
maetugr 40:2ca410923691 44 {
maetugr 40:2ca410923691 45 char buffer[6]; // 8-Bit pieces of axis data
maetugr 40:2ca410923691 46
maetugr 40:2ca410923691 47 readMultiRegister(MPU6050_RA_ACCEL_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7)
maetugr 40:2ca410923691 48
maetugr 40:2ca410923691 49 raw_acc[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
maetugr 40:2ca410923691 50 raw_acc[1] = (short) (buffer[2] << 8 | buffer[3]);
maetugr 40:2ca410923691 51 raw_acc[2] = (short) (buffer[4] << 8 | buffer[5]);
maetugr 40:2ca410923691 52 }
maetugr 40:2ca410923691 53
maetugr 40:2ca410923691 54 void MPU6050::calibrate(int times, float separation_time)
maetugr 40:2ca410923691 55 {
maetugr 40:2ca410923691 56 // calibrate sensor with an average of count samples (result of calibration stored in offset[])
maetugr 40:2ca410923691 57 // Calibrate Gyroscope ----------------------------------
maetugr 40:2ca410923691 58 float calib_gyro[3] = {0,0,0}; // temporary array for the sum of calibration measurement
maetugr 40:2ca410923691 59
maetugr 40:2ca410923691 60 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
maetugr 40:2ca410923691 61 readraw_gyro();
maetugr 40:2ca410923691 62 for (int j = 0; j < 3; j++)
maetugr 40:2ca410923691 63 calib_gyro[j] += raw_gyro[j];
maetugr 40:2ca410923691 64 wait(separation_time);
maetugr 40:2ca410923691 65 }
maetugr 40:2ca410923691 66
maetugr 40:2ca410923691 67 for (int i = 0; i < 3; i++)
maetugr 40:2ca410923691 68 offset_gyro[i] = calib_gyro[i]/times; // take the average of the calibration measurements
maetugr 40:2ca410923691 69
maetugr 40:2ca410923691 70 // Calibrate Accelerometer -------------------------------
maetugr 40:2ca410923691 71 float calib_acc[3] = {0,0,0}; // temporary array for the sum of calibration measurement
maetugr 40:2ca410923691 72
maetugr 40:2ca410923691 73 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
maetugr 40:2ca410923691 74 readraw_acc();
maetugr 40:2ca410923691 75 for (int j = 0; j < 3; j++)
maetugr 40:2ca410923691 76 calib_acc[j] += raw_acc[j];
maetugr 40:2ca410923691 77 wait(separation_time);
maetugr 40:2ca410923691 78 }
maetugr 40:2ca410923691 79
maetugr 40:2ca410923691 80 for (int i = 0; i < 2; i++)
maetugr 40:2ca410923691 81 offset_acc[i] = calib_acc[i]/times; // take the average of the calibration measurements
maetugr 40:2ca410923691 82 }