MPU6050 library using i2c interface on LPC1768 - Complementary filter is added. Now program can calculate pitch and roll angles.
Fork of MPU6050 by
MPU6050.cpp@7:e4e02c9c1381, 2016-11-03 (annotated)
- Committer:
- kfforex
- Date:
- Thu Nov 03 17:14:54 2016 +0000
- Revision:
- 7:e4e02c9c1381
- Parent:
- 6:5b90f2b5e6d9
before publishing
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BaserK | 4:20f1f660e5c3 | 1 | /* MPU6050 Library |
BaserK | 4:20f1f660e5c3 | 2 | * |
BaserK | 4:20f1f660e5c3 | 3 | * @author: Baser Kandehir |
BaserK | 2:3e0dfce73a58 | 4 | * @date: July 16, 2015 |
BaserK | 3:a173ad187e67 | 5 | * @license: MIT license |
BaserK | 0:954f15bd95f1 | 6 | */ |
BaserK | 0:954f15bd95f1 | 7 | |
BaserK | 0:954f15bd95f1 | 8 | |
BaserK | 0:954f15bd95f1 | 9 | #include "MPU6050.h" |
BaserK | 0:954f15bd95f1 | 10 | |
BaserK | 4:20f1f660e5c3 | 11 | /* For NUCLEO-F411RE board */ |
kfforex | 7:e4e02c9c1381 | 12 | static I2C i2c(D4,D5); // setup i2c (SDA,SCL) |
BaserK | 0:954f15bd95f1 | 13 | |
BaserK | 0:954f15bd95f1 | 14 | /* Set initial input parameters */ |
BaserK | 0:954f15bd95f1 | 15 | |
BaserK | 0:954f15bd95f1 | 16 | // Acc Full Scale Range +-2G 4G 8G 16G |
BaserK | 0:954f15bd95f1 | 17 | enum Ascale |
BaserK | 0:954f15bd95f1 | 18 | { |
BaserK | 0:954f15bd95f1 | 19 | AFS_2G=0, |
BaserK | 0:954f15bd95f1 | 20 | AFS_4G, |
BaserK | 0:954f15bd95f1 | 21 | AFS_8G, |
BaserK | 0:954f15bd95f1 | 22 | AFS_16G |
BaserK | 0:954f15bd95f1 | 23 | }; |
BaserK | 0:954f15bd95f1 | 24 | |
BaserK | 0:954f15bd95f1 | 25 | // Gyro Full Scale Range +-250 500 1000 2000 Degrees per second |
BaserK | 0:954f15bd95f1 | 26 | enum Gscale |
BaserK | 0:954f15bd95f1 | 27 | { |
BaserK | 0:954f15bd95f1 | 28 | GFS_250DPS=0, |
BaserK | 0:954f15bd95f1 | 29 | GFS_500DPS, |
BaserK | 0:954f15bd95f1 | 30 | GFS_1000DPS, |
BaserK | 0:954f15bd95f1 | 31 | GFS_2000DPS |
BaserK | 0:954f15bd95f1 | 32 | }; |
BaserK | 0:954f15bd95f1 | 33 | |
BaserK | 0:954f15bd95f1 | 34 | // Sensor datas |
BaserK | 0:954f15bd95f1 | 35 | float ax,ay,az; |
BaserK | 0:954f15bd95f1 | 36 | float gx,gy,gz; |
BaserK | 0:954f15bd95f1 | 37 | int16_t accelData[3],gyroData[3],tempData; |
BaserK | 0:954f15bd95f1 | 38 | float accelBias[3] = {0, 0, 0}; // Bias corrections for acc |
BaserK | 0:954f15bd95f1 | 39 | float gyroBias[3] = {0, 0, 0}; // Bias corrections for gyro |
BaserK | 0:954f15bd95f1 | 40 | |
BaserK | 0:954f15bd95f1 | 41 | // Specify sensor full scale range |
BaserK | 0:954f15bd95f1 | 42 | int Ascale = AFS_2G; |
BaserK | 0:954f15bd95f1 | 43 | int Gscale = GFS_250DPS; |
BaserK | 0:954f15bd95f1 | 44 | |
BaserK | 0:954f15bd95f1 | 45 | // Scale resolutions per LSB for the sensors |
BaserK | 0:954f15bd95f1 | 46 | float aRes, gRes; |
BaserK | 0:954f15bd95f1 | 47 | |
BaserK | 0:954f15bd95f1 | 48 | // Calculates Acc resolution |
BaserK | 0:954f15bd95f1 | 49 | void MPU6050::getAres() |
BaserK | 0:954f15bd95f1 | 50 | { |
BaserK | 0:954f15bd95f1 | 51 | switch(Ascale) |
BaserK | 0:954f15bd95f1 | 52 | { |
BaserK | 0:954f15bd95f1 | 53 | case AFS_2G: |
BaserK | 0:954f15bd95f1 | 54 | aRes = 2.0/32768.0; |
BaserK | 0:954f15bd95f1 | 55 | break; |
BaserK | 0:954f15bd95f1 | 56 | case AFS_4G: |
BaserK | 0:954f15bd95f1 | 57 | aRes = 4.0/32768.0; |
BaserK | 0:954f15bd95f1 | 58 | break; |
BaserK | 0:954f15bd95f1 | 59 | case AFS_8G: |
BaserK | 0:954f15bd95f1 | 60 | aRes = 8.0/32768.0; |
BaserK | 0:954f15bd95f1 | 61 | break; |
BaserK | 0:954f15bd95f1 | 62 | case AFS_16G: |
BaserK | 0:954f15bd95f1 | 63 | aRes = 16.0/32768.0; |
BaserK | 0:954f15bd95f1 | 64 | break; |
BaserK | 0:954f15bd95f1 | 65 | } |
BaserK | 0:954f15bd95f1 | 66 | } |
BaserK | 0:954f15bd95f1 | 67 | |
BaserK | 0:954f15bd95f1 | 68 | // Calculates Gyro resolution |
BaserK | 0:954f15bd95f1 | 69 | void MPU6050::getGres() |
BaserK | 0:954f15bd95f1 | 70 | { |
BaserK | 0:954f15bd95f1 | 71 | switch(Gscale) |
BaserK | 0:954f15bd95f1 | 72 | { |
BaserK | 0:954f15bd95f1 | 73 | case GFS_250DPS: |
BaserK | 0:954f15bd95f1 | 74 | gRes = 250.0/32768.0; |
BaserK | 0:954f15bd95f1 | 75 | break; |
BaserK | 0:954f15bd95f1 | 76 | case GFS_500DPS: |
BaserK | 0:954f15bd95f1 | 77 | gRes = 500.0/32768.0; |
BaserK | 0:954f15bd95f1 | 78 | break; |
BaserK | 0:954f15bd95f1 | 79 | case GFS_1000DPS: |
BaserK | 0:954f15bd95f1 | 80 | gRes = 1000.0/32768.0; |
BaserK | 0:954f15bd95f1 | 81 | break; |
BaserK | 0:954f15bd95f1 | 82 | case GFS_2000DPS: |
BaserK | 0:954f15bd95f1 | 83 | gRes = 2000.0/32768.0; |
BaserK | 0:954f15bd95f1 | 84 | break; |
BaserK | 0:954f15bd95f1 | 85 | } |
BaserK | 0:954f15bd95f1 | 86 | } |
BaserK | 0:954f15bd95f1 | 87 | |
BaserK | 0:954f15bd95f1 | 88 | void MPU6050::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) |
BaserK | 0:954f15bd95f1 | 89 | { |
BaserK | 0:954f15bd95f1 | 90 | char data_write[2]; |
BaserK | 0:954f15bd95f1 | 91 | data_write[0]=subAddress; // I2C sends MSB first. Namely >>|subAddress|>>|data| |
BaserK | 0:954f15bd95f1 | 92 | data_write[1]=data; |
BaserK | 0:954f15bd95f1 | 93 | i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false); |
BaserK | 0:954f15bd95f1 | 94 | } |
BaserK | 0:954f15bd95f1 | 95 | |
BaserK | 0:954f15bd95f1 | 96 | char MPU6050::readByte(uint8_t address, uint8_t subAddress) |
BaserK | 0:954f15bd95f1 | 97 | { |
BaserK | 0:954f15bd95f1 | 98 | char data_read[1]; // will store the register data |
BaserK | 0:954f15bd95f1 | 99 | char data_write[1]; |
BaserK | 0:954f15bd95f1 | 100 | data_write[0]=subAddress; |
BaserK | 0:954f15bd95f1 | 101 | i2c.write(address,data_write,1,1); // have not stopped yet |
BaserK | 0:954f15bd95f1 | 102 | i2c.read(address,data_read,1,0); // read the data and stop |
BaserK | 0:954f15bd95f1 | 103 | return data_read[0]; |
BaserK | 0:954f15bd95f1 | 104 | } |
BaserK | 0:954f15bd95f1 | 105 | |
BaserK | 0:954f15bd95f1 | 106 | void MPU6050::readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest) |
BaserK | 0:954f15bd95f1 | 107 | { |
BaserK | 0:954f15bd95f1 | 108 | char data[14],data_write[1]; |
BaserK | 0:954f15bd95f1 | 109 | data_write[0]=subAddress; |
BaserK | 0:954f15bd95f1 | 110 | i2c.write(address,data_write,1,1); |
BaserK | 0:954f15bd95f1 | 111 | i2c.read(address,data,byteNum,0); |
BaserK | 0:954f15bd95f1 | 112 | for(int i=0;i<byteNum;i++) // equate the addresses |
BaserK | 0:954f15bd95f1 | 113 | dest[i]=data[i]; |
BaserK | 0:954f15bd95f1 | 114 | } |
BaserK | 0:954f15bd95f1 | 115 | |
BaserK | 0:954f15bd95f1 | 116 | // Communication test: WHO_AM_I register reading |
BaserK | 0:954f15bd95f1 | 117 | void MPU6050::whoAmI() |
BaserK | 0:954f15bd95f1 | 118 | { |
BaserK | 0:954f15bd95f1 | 119 | uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Should return 0x68 |
BaserK | 2:3e0dfce73a58 | 120 | pc.printf("I AM 0x%x \r\n",whoAmI); |
BaserK | 0:954f15bd95f1 | 121 | |
BaserK | 0:954f15bd95f1 | 122 | if(whoAmI==0x68) |
BaserK | 0:954f15bd95f1 | 123 | { |
BaserK | 2:3e0dfce73a58 | 124 | pc.printf("MPU6050 is online... \r\n"); |
BaserK | 0:954f15bd95f1 | 125 | } |
BaserK | 0:954f15bd95f1 | 126 | else |
BaserK | 0:954f15bd95f1 | 127 | { |
BaserK | 2:3e0dfce73a58 | 128 | pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n"); |
BaserK | 0:954f15bd95f1 | 129 | } |
BaserK | 0:954f15bd95f1 | 130 | } |
BaserK | 0:954f15bd95f1 | 131 | |
BaserK | 0:954f15bd95f1 | 132 | // Initializes MPU6050 with the following config: |
BaserK | 0:954f15bd95f1 | 133 | // PLL with X axis gyroscope reference |
BaserK | 0:954f15bd95f1 | 134 | // Sample rate: 200Hz for gyro and acc |
BaserK | 0:954f15bd95f1 | 135 | // Interrupts are disabled |
BaserK | 0:954f15bd95f1 | 136 | void MPU6050::init() |
BaserK | 5:5bff0edcdff8 | 137 | { |
BaserK | 5:5bff0edcdff8 | 138 | i2c.frequency(400000); // fast i2c: 400 kHz |
BaserK | 5:5bff0edcdff8 | 139 | |
BaserK | 0:954f15bd95f1 | 140 | /* Wake up the device */ |
BaserK | 0:954f15bd95f1 | 141 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // wake up the device by clearing the sleep bit (bit6) |
BaserK | 0:954f15bd95f1 | 142 | wait_ms(100); // wait 100 ms to stabilize |
BaserK | 0:954f15bd95f1 | 143 | |
BaserK | 0:954f15bd95f1 | 144 | /* Get stable time source */ |
BaserK | 0:954f15bd95f1 | 145 | // PLL with X axis gyroscope reference is used to improve stability |
BaserK | 0:954f15bd95f1 | 146 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); |
BaserK | 0:954f15bd95f1 | 147 | |
BaserK | 0:954f15bd95f1 | 148 | /* Configure Gyroscope and Accelerometer */ |
BaserK | 0:954f15bd95f1 | 149 | // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz |
BaserK | 0:954f15bd95f1 | 150 | // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate) |
BaserK | 0:954f15bd95f1 | 151 | writeByte(MPU6050_ADDRESS, CONFIG, 0x03); |
BaserK | 0:954f15bd95f1 | 152 | |
BaserK | 0:954f15bd95f1 | 153 | /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */ |
BaserK | 0:954f15bd95f1 | 154 | // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above) |
BaserK | 0:954f15bd95f1 | 155 | writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); |
BaserK | 0:954f15bd95f1 | 156 | |
BaserK | 0:954f15bd95f1 | 157 | /* Accelerometer configuration */ |
BaserK | 0:954f15bd95f1 | 158 | uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); |
BaserK | 0:954f15bd95f1 | 159 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] |
BaserK | 0:954f15bd95f1 | 160 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0x18); // Clear AFS bits [4:3] |
BaserK | 0:954f15bd95f1 | 161 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp | Ascale<<3); // Set full scale range |
BaserK | 0:954f15bd95f1 | 162 | |
BaserK | 0:954f15bd95f1 | 163 | /* Gyroscope configuration */ |
BaserK | 0:954f15bd95f1 | 164 | temp = readByte(MPU6050_ADDRESS, GYRO_CONFIG); |
BaserK | 0:954f15bd95f1 | 165 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] |
BaserK | 0:954f15bd95f1 | 166 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18); // Clear FS bits [4:3] |
BaserK | 0:954f15bd95f1 | 167 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3); // Set full scale range |
BaserK | 0:954f15bd95f1 | 168 | } |
BaserK | 0:954f15bd95f1 | 169 | |
BaserK | 0:954f15bd95f1 | 170 | // Resets the device |
BaserK | 0:954f15bd95f1 | 171 | void MPU6050::reset() |
BaserK | 0:954f15bd95f1 | 172 | { |
BaserK | 0:954f15bd95f1 | 173 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // set bit7 to reset the device |
BaserK | 0:954f15bd95f1 | 174 | wait_ms(100); // wait 100 ms to stabilize |
BaserK | 0:954f15bd95f1 | 175 | } |
BaserK | 0:954f15bd95f1 | 176 | |
BaserK | 0:954f15bd95f1 | 177 | void MPU6050::readAccelData(int16_t* dest) |
BaserK | 0:954f15bd95f1 | 178 | { |
BaserK | 0:954f15bd95f1 | 179 | uint8_t rawData[6]; // x,y,z acc data |
BaserK | 0:954f15bd95f1 | 180 | readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // read six raw data registers sequentially and write them into data array |
BaserK | 0:954f15bd95f1 | 181 | |
BaserK | 0:954f15bd95f1 | 182 | /* Turn the MSB LSB into signed 16-bit value */ |
BaserK | 0:954f15bd95f1 | 183 | dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // ACCEL_XOUT |
BaserK | 0:954f15bd95f1 | 184 | dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // ACCEL_YOUT |
BaserK | 0:954f15bd95f1 | 185 | dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // ACCEL_ZOUT |
BaserK | 0:954f15bd95f1 | 186 | } |
BaserK | 0:954f15bd95f1 | 187 | |
BaserK | 0:954f15bd95f1 | 188 | void MPU6050::readGyroData(int16_t* dest) |
BaserK | 0:954f15bd95f1 | 189 | { |
BaserK | 0:954f15bd95f1 | 190 | uint8_t rawData[6]; // x,y,z gyro data |
BaserK | 0:954f15bd95f1 | 191 | readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // read the six raw data registers sequentially and write them into data array |
BaserK | 0:954f15bd95f1 | 192 | |
BaserK | 0:954f15bd95f1 | 193 | /* Turn the MSB LSB into signed 16-bit value */ |
BaserK | 0:954f15bd95f1 | 194 | dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // GYRO_XOUT |
BaserK | 0:954f15bd95f1 | 195 | dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // GYRO_YOUT |
BaserK | 0:954f15bd95f1 | 196 | dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // GYRO_ZOUT |
BaserK | 0:954f15bd95f1 | 197 | } |
BaserK | 0:954f15bd95f1 | 198 | |
BaserK | 0:954f15bd95f1 | 199 | int16_t MPU6050::readTempData() |
BaserK | 0:954f15bd95f1 | 200 | { |
BaserK | 0:954f15bd95f1 | 201 | uint8_t rawData[2]; // temperature data |
BaserK | 0:954f15bd95f1 | 202 | readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // read the two raw data registers sequentially and write them into data array |
BaserK | 0:954f15bd95f1 | 203 | return (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // turn the MSB LSB into signed 16-bit value |
BaserK | 0:954f15bd95f1 | 204 | } |
BaserK | 0:954f15bd95f1 | 205 | |
BaserK | 0:954f15bd95f1 | 206 | /* Function which accumulates gyro and accelerometer data after device initialization. |
BaserK | 0:954f15bd95f1 | 207 | It calculates the average of the at-rest readings and |
BaserK | 0:954f15bd95f1 | 208 | then loads the resulting offsets into accelerometer and gyro bias registers. */ |
BaserK | 0:954f15bd95f1 | 209 | /* |
BaserK | 0:954f15bd95f1 | 210 | IMPORTANT NOTE: In this function; |
BaserK | 0:954f15bd95f1 | 211 | Resulting accel offsets are NOT pushed to the accel bias registers. accelBias[i] offsets are used in the main program. |
BaserK | 0:954f15bd95f1 | 212 | Resulting gyro offsets are pushed to the gyro bias registers. gyroBias[i] offsets are NOT used in the main program. |
BaserK | 0:954f15bd95f1 | 213 | Resulting data seems satisfactory. |
BaserK | 0:954f15bd95f1 | 214 | */ |
BaserK | 0:954f15bd95f1 | 215 | // dest1: accelBias dest2: gyroBias |
BaserK | 0:954f15bd95f1 | 216 | void MPU6050::calibrate(float* dest1, float* dest2) |
BaserK | 0:954f15bd95f1 | 217 | { |
BaserK | 0:954f15bd95f1 | 218 | uint8_t data[12]; // data array to hold acc and gyro x,y,z data |
BaserK | 0:954f15bd95f1 | 219 | uint16_t fifo_count, packet_count, count; |
BaserK | 0:954f15bd95f1 | 220 | int32_t accel_bias[3] = {0,0,0}; |
BaserK | 0:954f15bd95f1 | 221 | int32_t gyro_bias[3] = {0,0,0}; |
BaserK | 0:954f15bd95f1 | 222 | float aRes = 2.0/32768.0; |
BaserK | 0:954f15bd95f1 | 223 | float gRes = 250.0/32768.0; |
BaserK | 0:954f15bd95f1 | 224 | uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g |
BaserK | 0:954f15bd95f1 | 225 | //uint16_t gyrosensitivity = 131; // = 1/gRes = 131 LSB/dps |
BaserK | 0:954f15bd95f1 | 226 | |
BaserK | 0:954f15bd95f1 | 227 | reset(); // Reset device |
BaserK | 0:954f15bd95f1 | 228 | |
BaserK | 0:954f15bd95f1 | 229 | /* Get stable time source */ |
BaserK | 0:954f15bd95f1 | 230 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // PLL with X axis gyroscope reference is used to improve stability |
BaserK | 0:954f15bd95f1 | 231 | writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); // Disable accel only low power mode |
BaserK | 0:954f15bd95f1 | 232 | wait(0.2); |
BaserK | 0:954f15bd95f1 | 233 | |
BaserK | 0:954f15bd95f1 | 234 | /* Configure device for bias calculation */ |
BaserK | 0:954f15bd95f1 | 235 | writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts |
BaserK | 0:954f15bd95f1 | 236 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO |
BaserK | 0:954f15bd95f1 | 237 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source |
BaserK | 0:954f15bd95f1 | 238 | writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master |
BaserK | 0:954f15bd95f1 | 239 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes |
BaserK | 0:954f15bd95f1 | 240 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x04); // Reset FIFO |
BaserK | 0:954f15bd95f1 | 241 | wait(0.015); |
BaserK | 0:954f15bd95f1 | 242 | |
BaserK | 0:954f15bd95f1 | 243 | /* Configure accel and gyro for bias calculation */ |
BaserK | 0:954f15bd95f1 | 244 | writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz |
BaserK | 0:954f15bd95f1 | 245 | writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz |
BaserK | 0:954f15bd95f1 | 246 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity |
BaserK | 0:954f15bd95f1 | 247 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity |
BaserK | 0:954f15bd95f1 | 248 | |
BaserK | 0:954f15bd95f1 | 249 | /* Configure FIFO to capture accelerometer and gyro data for bias calculation */ |
BaserK | 0:954f15bd95f1 | 250 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO |
BaserK | 0:954f15bd95f1 | 251 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable accelerometer and gyro for FIFO (max size 1024 bytes in MPU-6050) |
BaserK | 0:954f15bd95f1 | 252 | wait(0.08); // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds. |
BaserK | 0:954f15bd95f1 | 253 | // accX: 2 byte, accY: 2 byte, accZ: 2 byte. gyroX: 2 byte, gyroY: 2 byte, gyroZ: 2 byte. 12*80=960 byte < 1024 byte |
BaserK | 0:954f15bd95f1 | 254 | |
BaserK | 0:954f15bd95f1 | 255 | /* At end of sample accumulation, turn off FIFO sensor read */ |
BaserK | 0:954f15bd95f1 | 256 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO |
BaserK | 0:954f15bd95f1 | 257 | readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // Read FIFO sample count |
BaserK | 0:954f15bd95f1 | 258 | fifo_count = ((uint16_t)data[0] << 8) | data[1]; |
BaserK | 0:954f15bd95f1 | 259 | packet_count = fifo_count/12; // The number of sets of full acc and gyro data for averaging. packet_count = 80 in this case |
BaserK | 0:954f15bd95f1 | 260 | |
BaserK | 0:954f15bd95f1 | 261 | for(count=0; count<packet_count; count++) |
BaserK | 0:954f15bd95f1 | 262 | { |
BaserK | 0:954f15bd95f1 | 263 | int16_t accel_temp[3]={0,0,0}; |
BaserK | 0:954f15bd95f1 | 264 | int16_t gyro_temp[3]={0,0,0}; |
BaserK | 0:954f15bd95f1 | 265 | readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging |
BaserK | 0:954f15bd95f1 | 266 | |
BaserK | 0:954f15bd95f1 | 267 | /* Form signed 16-bit integer for each sample in FIFO */ |
BaserK | 0:954f15bd95f1 | 268 | accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; |
BaserK | 0:954f15bd95f1 | 269 | accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; |
BaserK | 0:954f15bd95f1 | 270 | accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; |
BaserK | 0:954f15bd95f1 | 271 | gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; |
BaserK | 0:954f15bd95f1 | 272 | gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; |
BaserK | 0:954f15bd95f1 | 273 | gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; |
BaserK | 0:954f15bd95f1 | 274 | |
BaserK | 0:954f15bd95f1 | 275 | /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */ |
BaserK | 0:954f15bd95f1 | 276 | accel_bias[0] += (int32_t) accel_temp[0]; |
BaserK | 0:954f15bd95f1 | 277 | accel_bias[1] += (int32_t) accel_temp[1]; |
BaserK | 0:954f15bd95f1 | 278 | accel_bias[2] += (int32_t) accel_temp[2]; |
BaserK | 0:954f15bd95f1 | 279 | gyro_bias[0] += (int32_t) gyro_temp[0]; |
BaserK | 0:954f15bd95f1 | 280 | gyro_bias[1] += (int32_t) gyro_temp[1]; |
BaserK | 0:954f15bd95f1 | 281 | gyro_bias[2] += (int32_t) gyro_temp[2]; |
BaserK | 0:954f15bd95f1 | 282 | } |
BaserK | 0:954f15bd95f1 | 283 | |
BaserK | 0:954f15bd95f1 | 284 | /* Normalize sums to get average count biases */ |
BaserK | 0:954f15bd95f1 | 285 | accel_bias[0] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 286 | accel_bias[1] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 287 | accel_bias[2] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 288 | gyro_bias[0] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 289 | gyro_bias[1] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 290 | gyro_bias[2] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 291 | |
BaserK | 0:954f15bd95f1 | 292 | /* Remove gravity from the z-axis accelerometer bias calculation */ |
BaserK | 0:954f15bd95f1 | 293 | if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;} |
BaserK | 0:954f15bd95f1 | 294 | else {accel_bias[2] += (int32_t) accelsensitivity;} |
BaserK | 0:954f15bd95f1 | 295 | |
BaserK | 0:954f15bd95f1 | 296 | /* Output scaled accelerometer biases for manual subtraction in the main program */ |
BaserK | 0:954f15bd95f1 | 297 | dest1[0] = accel_bias[0]*aRes; |
BaserK | 0:954f15bd95f1 | 298 | dest1[1] = accel_bias[1]*aRes; |
BaserK | 0:954f15bd95f1 | 299 | dest1[2] = accel_bias[2]*aRes; |
BaserK | 0:954f15bd95f1 | 300 | |
BaserK | 0:954f15bd95f1 | 301 | /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */ |
BaserK | 0:954f15bd95f1 | 302 | data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format |
BaserK | 0:954f15bd95f1 | 303 | data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases |
BaserK | 0:954f15bd95f1 | 304 | data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; |
BaserK | 0:954f15bd95f1 | 305 | data[3] = (-gyro_bias[1]/4) & 0xFF; |
BaserK | 0:954f15bd95f1 | 306 | data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; |
BaserK | 0:954f15bd95f1 | 307 | data[5] = (-gyro_bias[2]/4) & 0xFF; |
BaserK | 0:954f15bd95f1 | 308 | |
BaserK | 0:954f15bd95f1 | 309 | /* Push gyro biases to hardware registers */ |
BaserK | 0:954f15bd95f1 | 310 | writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); |
BaserK | 0:954f15bd95f1 | 311 | writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]); |
BaserK | 0:954f15bd95f1 | 312 | writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]); |
BaserK | 0:954f15bd95f1 | 313 | writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]); |
BaserK | 0:954f15bd95f1 | 314 | writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]); |
BaserK | 0:954f15bd95f1 | 315 | writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]); |
BaserK | 0:954f15bd95f1 | 316 | |
BaserK | 0:954f15bd95f1 | 317 | /* Construct gyro bias in deg/s for later manual subtraction */ |
BaserK | 0:954f15bd95f1 | 318 | dest2[0] = gyro_bias[0]*gRes; |
BaserK | 0:954f15bd95f1 | 319 | dest2[1] = gyro_bias[1]*gRes; |
BaserK | 0:954f15bd95f1 | 320 | dest2[2] = gyro_bias[2]*gRes; |
BaserK | 0:954f15bd95f1 | 321 | } |
BaserK | 2:3e0dfce73a58 | 322 | |
BaserK | 2:3e0dfce73a58 | 323 | void MPU6050::complementaryFilter(float* pitch, float* roll) |
BaserK | 2:3e0dfce73a58 | 324 | { |
BaserK | 2:3e0dfce73a58 | 325 | /* Get actual acc value */ |
BaserK | 2:3e0dfce73a58 | 326 | readAccelData(accelData); |
BaserK | 2:3e0dfce73a58 | 327 | getAres(); |
BaserK | 2:3e0dfce73a58 | 328 | ax = accelData[0]*aRes - accelBias[0]; |
BaserK | 2:3e0dfce73a58 | 329 | ay = accelData[1]*aRes - accelBias[1]; |
BaserK | 2:3e0dfce73a58 | 330 | az = accelData[2]*aRes - accelBias[2]; |
BaserK | 2:3e0dfce73a58 | 331 | |
BaserK | 2:3e0dfce73a58 | 332 | /* Get actual gyro value */ |
BaserK | 2:3e0dfce73a58 | 333 | readGyroData(gyroData); |
BaserK | 2:3e0dfce73a58 | 334 | getGres(); |
kfforex | 7:e4e02c9c1381 | 335 | gx = gyroData[0]*gRes; // Results are better without extracting gyroBias[i] |
kfforex | 7:e4e02c9c1381 | 336 | gy = gyroData[1]*gRes; |
kfforex | 7:e4e02c9c1381 | 337 | gz = gyroData[2]*gRes; |
BaserK | 2:3e0dfce73a58 | 338 | |
BaserK | 2:3e0dfce73a58 | 339 | float pitchAcc, rollAcc; |
BaserK | 2:3e0dfce73a58 | 340 | |
BaserK | 2:3e0dfce73a58 | 341 | /* Integrate the gyro data(deg/s) over time to get angle */ |
BaserK | 2:3e0dfce73a58 | 342 | *pitch += gx * dt; // Angle around the X-axis |
BaserK | 2:3e0dfce73a58 | 343 | *roll -= gy * dt; // Angle around the Y-axis |
BaserK | 2:3e0dfce73a58 | 344 | |
BaserK | 2:3e0dfce73a58 | 345 | /* Turning around the X-axis results in a vector on the Y-axis |
BaserK | 2:3e0dfce73a58 | 346 | whereas turning around the Y-axis results in a vector on the X-axis. */ |
BaserK | 2:3e0dfce73a58 | 347 | pitchAcc = atan2f(accelData[1], accelData[2])*180/PI; |
BaserK | 2:3e0dfce73a58 | 348 | rollAcc = atan2f(accelData[0], accelData[2])*180/PI; |
BaserK | 2:3e0dfce73a58 | 349 | |
BaserK | 2:3e0dfce73a58 | 350 | /* Apply Complementary Filter */ |
BaserK | 2:3e0dfce73a58 | 351 | *pitch = *pitch * 0.98 + pitchAcc * 0.02; |
BaserK | 2:3e0dfce73a58 | 352 | *roll = *roll * 0.98 + rollAcc * 0.02; |
BaserK | 2:3e0dfce73a58 | 353 | } |