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