Matthias Grob / Mbed 2 deprecated FlyBed3

Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)

Dependencies:   mbed

IMU/MPU6050/MPU6050.cpp@8:609a2ad4c30e, 2015-11-19 (annotated)

Committer:
maetugr
Date:
Thu Nov 19 18:47:27 2015 +0000
Revision:
8:609a2ad4c30e
made I2C-Sensors working in parallel, added rolling buffer for PID derivative, played with the PID and frequency parameters in main

Who changed what in which revision?

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