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.
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?
User | Revision | Line number | New 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 | } |