a
Fork of MPU6050 by
MPU6050.cpp@5:5bff0edcdff8, 2015-08-05 (annotated)
- Committer:
- BaserK
- Date:
- Wed Aug 05 12:45:47 2015 +0000
- Revision:
- 5:5bff0edcdff8
- Parent:
- 4:20f1f660e5c3
- Child:
- 6:5b90f2b5e6d9
i2c object is no more extern
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 | 3:a173ad187e67 | 6 | * |
BaserK | 3:a173ad187e67 | 7 | * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr |
BaserK | 3:a173ad187e67 | 8 | * |
BaserK | 3:a173ad187e67 | 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
BaserK | 3:a173ad187e67 | 10 | * of this software and associated documentation files (the "Software"), to deal |
BaserK | 3:a173ad187e67 | 11 | * in the Software without restriction, including without limitation the rights |
BaserK | 3:a173ad187e67 | 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
BaserK | 3:a173ad187e67 | 13 | * copies of the Software, and to permit persons to whom the Software is |
BaserK | 3:a173ad187e67 | 14 | * furnished to do so, subject to the following conditions: |
BaserK | 3:a173ad187e67 | 15 | * |
BaserK | 3:a173ad187e67 | 16 | * The above copyright notice and this permission notice shall be included in |
BaserK | 3:a173ad187e67 | 17 | * all copies or substantial portions of the Software. |
BaserK | 3:a173ad187e67 | 18 | * |
BaserK | 3:a173ad187e67 | 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
BaserK | 3:a173ad187e67 | 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
BaserK | 3:a173ad187e67 | 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
BaserK | 3:a173ad187e67 | 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
BaserK | 3:a173ad187e67 | 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
BaserK | 3:a173ad187e67 | 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
BaserK | 3:a173ad187e67 | 25 | * THE SOFTWARE. |
BaserK | 3:a173ad187e67 | 26 | * |
BaserK | 0:954f15bd95f1 | 27 | */ |
BaserK | 0:954f15bd95f1 | 28 | |
BaserK | 0:954f15bd95f1 | 29 | // Most of the code is adapted from Kris Winer's MPU6050 library |
BaserK | 0:954f15bd95f1 | 30 | |
BaserK | 0:954f15bd95f1 | 31 | #include "MPU6050.h" |
BaserK | 0:954f15bd95f1 | 32 | |
BaserK | 4:20f1f660e5c3 | 33 | /* For LPC1768 board */ |
BaserK | 4:20f1f660e5c3 | 34 | //I2C i2c(p9,p10); // setup i2c (SDA,SCL) |
BaserK | 4:20f1f660e5c3 | 35 | |
BaserK | 4:20f1f660e5c3 | 36 | /* For NUCLEO-F411RE board */ |
BaserK | 4:20f1f660e5c3 | 37 | I2C i2c(D14,D15); // setup i2c (SDA,SCL) |
BaserK | 0:954f15bd95f1 | 38 | |
BaserK | 0:954f15bd95f1 | 39 | /* Set initial input parameters */ |
BaserK | 0:954f15bd95f1 | 40 | |
BaserK | 0:954f15bd95f1 | 41 | // Acc Full Scale Range +-2G 4G 8G 16G |
BaserK | 0:954f15bd95f1 | 42 | enum Ascale |
BaserK | 0:954f15bd95f1 | 43 | { |
BaserK | 0:954f15bd95f1 | 44 | AFS_2G=0, |
BaserK | 0:954f15bd95f1 | 45 | AFS_4G, |
BaserK | 0:954f15bd95f1 | 46 | AFS_8G, |
BaserK | 0:954f15bd95f1 | 47 | AFS_16G |
BaserK | 0:954f15bd95f1 | 48 | }; |
BaserK | 0:954f15bd95f1 | 49 | |
BaserK | 0:954f15bd95f1 | 50 | // Gyro Full Scale Range +-250 500 1000 2000 Degrees per second |
BaserK | 0:954f15bd95f1 | 51 | enum Gscale |
BaserK | 0:954f15bd95f1 | 52 | { |
BaserK | 0:954f15bd95f1 | 53 | GFS_250DPS=0, |
BaserK | 0:954f15bd95f1 | 54 | GFS_500DPS, |
BaserK | 0:954f15bd95f1 | 55 | GFS_1000DPS, |
BaserK | 0:954f15bd95f1 | 56 | GFS_2000DPS |
BaserK | 0:954f15bd95f1 | 57 | }; |
BaserK | 0:954f15bd95f1 | 58 | |
BaserK | 0:954f15bd95f1 | 59 | // Sensor datas |
BaserK | 0:954f15bd95f1 | 60 | float ax,ay,az; |
BaserK | 0:954f15bd95f1 | 61 | float gx,gy,gz; |
BaserK | 0:954f15bd95f1 | 62 | int16_t accelData[3],gyroData[3],tempData; |
BaserK | 0:954f15bd95f1 | 63 | float accelBias[3] = {0, 0, 0}; // Bias corrections for acc |
BaserK | 0:954f15bd95f1 | 64 | float gyroBias[3] = {0, 0, 0}; // Bias corrections for gyro |
BaserK | 0:954f15bd95f1 | 65 | |
BaserK | 0:954f15bd95f1 | 66 | // Specify sensor full scale range |
BaserK | 0:954f15bd95f1 | 67 | int Ascale = AFS_2G; |
BaserK | 0:954f15bd95f1 | 68 | int Gscale = GFS_250DPS; |
BaserK | 0:954f15bd95f1 | 69 | |
BaserK | 0:954f15bd95f1 | 70 | // Scale resolutions per LSB for the sensors |
BaserK | 0:954f15bd95f1 | 71 | float aRes, gRes; |
BaserK | 0:954f15bd95f1 | 72 | |
BaserK | 0:954f15bd95f1 | 73 | // Calculates Acc resolution |
BaserK | 0:954f15bd95f1 | 74 | void MPU6050::getAres() |
BaserK | 0:954f15bd95f1 | 75 | { |
BaserK | 0:954f15bd95f1 | 76 | switch(Ascale) |
BaserK | 0:954f15bd95f1 | 77 | { |
BaserK | 0:954f15bd95f1 | 78 | case AFS_2G: |
BaserK | 0:954f15bd95f1 | 79 | aRes = 2.0/32768.0; |
BaserK | 0:954f15bd95f1 | 80 | break; |
BaserK | 0:954f15bd95f1 | 81 | case AFS_4G: |
BaserK | 0:954f15bd95f1 | 82 | aRes = 4.0/32768.0; |
BaserK | 0:954f15bd95f1 | 83 | break; |
BaserK | 0:954f15bd95f1 | 84 | case AFS_8G: |
BaserK | 0:954f15bd95f1 | 85 | aRes = 8.0/32768.0; |
BaserK | 0:954f15bd95f1 | 86 | break; |
BaserK | 0:954f15bd95f1 | 87 | case AFS_16G: |
BaserK | 0:954f15bd95f1 | 88 | aRes = 16.0/32768.0; |
BaserK | 0:954f15bd95f1 | 89 | break; |
BaserK | 0:954f15bd95f1 | 90 | } |
BaserK | 0:954f15bd95f1 | 91 | } |
BaserK | 0:954f15bd95f1 | 92 | |
BaserK | 0:954f15bd95f1 | 93 | // Calculates Gyro resolution |
BaserK | 0:954f15bd95f1 | 94 | void MPU6050::getGres() |
BaserK | 0:954f15bd95f1 | 95 | { |
BaserK | 0:954f15bd95f1 | 96 | switch(Gscale) |
BaserK | 0:954f15bd95f1 | 97 | { |
BaserK | 0:954f15bd95f1 | 98 | case GFS_250DPS: |
BaserK | 0:954f15bd95f1 | 99 | gRes = 250.0/32768.0; |
BaserK | 0:954f15bd95f1 | 100 | break; |
BaserK | 0:954f15bd95f1 | 101 | case GFS_500DPS: |
BaserK | 0:954f15bd95f1 | 102 | gRes = 500.0/32768.0; |
BaserK | 0:954f15bd95f1 | 103 | break; |
BaserK | 0:954f15bd95f1 | 104 | case GFS_1000DPS: |
BaserK | 0:954f15bd95f1 | 105 | gRes = 1000.0/32768.0; |
BaserK | 0:954f15bd95f1 | 106 | break; |
BaserK | 0:954f15bd95f1 | 107 | case GFS_2000DPS: |
BaserK | 0:954f15bd95f1 | 108 | gRes = 2000.0/32768.0; |
BaserK | 0:954f15bd95f1 | 109 | break; |
BaserK | 0:954f15bd95f1 | 110 | } |
BaserK | 0:954f15bd95f1 | 111 | } |
BaserK | 0:954f15bd95f1 | 112 | |
BaserK | 0:954f15bd95f1 | 113 | void MPU6050::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) |
BaserK | 0:954f15bd95f1 | 114 | { |
BaserK | 0:954f15bd95f1 | 115 | char data_write[2]; |
BaserK | 0:954f15bd95f1 | 116 | data_write[0]=subAddress; // I2C sends MSB first. Namely >>|subAddress|>>|data| |
BaserK | 0:954f15bd95f1 | 117 | data_write[1]=data; |
BaserK | 0:954f15bd95f1 | 118 | i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false); |
BaserK | 0:954f15bd95f1 | 119 | } |
BaserK | 0:954f15bd95f1 | 120 | |
BaserK | 0:954f15bd95f1 | 121 | char MPU6050::readByte(uint8_t address, uint8_t subAddress) |
BaserK | 0:954f15bd95f1 | 122 | { |
BaserK | 0:954f15bd95f1 | 123 | char data_read[1]; // will store the register data |
BaserK | 0:954f15bd95f1 | 124 | char data_write[1]; |
BaserK | 0:954f15bd95f1 | 125 | data_write[0]=subAddress; |
BaserK | 0:954f15bd95f1 | 126 | i2c.write(address,data_write,1,1); // have not stopped yet |
BaserK | 0:954f15bd95f1 | 127 | i2c.read(address,data_read,1,0); // read the data and stop |
BaserK | 0:954f15bd95f1 | 128 | return data_read[0]; |
BaserK | 0:954f15bd95f1 | 129 | } |
BaserK | 0:954f15bd95f1 | 130 | |
BaserK | 0:954f15bd95f1 | 131 | void MPU6050::readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest) |
BaserK | 0:954f15bd95f1 | 132 | { |
BaserK | 0:954f15bd95f1 | 133 | char data[14],data_write[1]; |
BaserK | 0:954f15bd95f1 | 134 | data_write[0]=subAddress; |
BaserK | 0:954f15bd95f1 | 135 | i2c.write(address,data_write,1,1); |
BaserK | 0:954f15bd95f1 | 136 | i2c.read(address,data,byteNum,0); |
BaserK | 0:954f15bd95f1 | 137 | for(int i=0;i<byteNum;i++) // equate the addresses |
BaserK | 0:954f15bd95f1 | 138 | dest[i]=data[i]; |
BaserK | 0:954f15bd95f1 | 139 | } |
BaserK | 0:954f15bd95f1 | 140 | |
BaserK | 0:954f15bd95f1 | 141 | // Communication test: WHO_AM_I register reading |
BaserK | 0:954f15bd95f1 | 142 | void MPU6050::whoAmI() |
BaserK | 0:954f15bd95f1 | 143 | { |
BaserK | 0:954f15bd95f1 | 144 | uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Should return 0x68 |
BaserK | 2:3e0dfce73a58 | 145 | pc.printf("I AM 0x%x \r\n",whoAmI); |
BaserK | 0:954f15bd95f1 | 146 | |
BaserK | 0:954f15bd95f1 | 147 | if(whoAmI==0x68) |
BaserK | 0:954f15bd95f1 | 148 | { |
BaserK | 2:3e0dfce73a58 | 149 | pc.printf("MPU6050 is online... \r\n"); |
BaserK | 0:954f15bd95f1 | 150 | led2=1; |
BaserK | 0:954f15bd95f1 | 151 | } |
BaserK | 0:954f15bd95f1 | 152 | else |
BaserK | 0:954f15bd95f1 | 153 | { |
BaserK | 2:3e0dfce73a58 | 154 | pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n"); |
BaserK | 0:954f15bd95f1 | 155 | toggler1.attach(&toggle_led1,0.1); // toggles led1 every 100 ms |
BaserK | 0:954f15bd95f1 | 156 | } |
BaserK | 0:954f15bd95f1 | 157 | } |
BaserK | 0:954f15bd95f1 | 158 | |
BaserK | 0:954f15bd95f1 | 159 | // Initializes MPU6050 with the following config: |
BaserK | 0:954f15bd95f1 | 160 | // PLL with X axis gyroscope reference |
BaserK | 0:954f15bd95f1 | 161 | // Sample rate: 200Hz for gyro and acc |
BaserK | 0:954f15bd95f1 | 162 | // Interrupts are disabled |
BaserK | 0:954f15bd95f1 | 163 | void MPU6050::init() |
BaserK | 5:5bff0edcdff8 | 164 | { |
BaserK | 5:5bff0edcdff8 | 165 | i2c.frequency(400000); // fast i2c: 400 kHz |
BaserK | 5:5bff0edcdff8 | 166 | |
BaserK | 0:954f15bd95f1 | 167 | /* Wake up the device */ |
BaserK | 0:954f15bd95f1 | 168 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // wake up the device by clearing the sleep bit (bit6) |
BaserK | 0:954f15bd95f1 | 169 | wait_ms(100); // wait 100 ms to stabilize |
BaserK | 0:954f15bd95f1 | 170 | |
BaserK | 0:954f15bd95f1 | 171 | /* Get stable time source */ |
BaserK | 0:954f15bd95f1 | 172 | // PLL with X axis gyroscope reference is used to improve stability |
BaserK | 0:954f15bd95f1 | 173 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); |
BaserK | 0:954f15bd95f1 | 174 | |
BaserK | 0:954f15bd95f1 | 175 | /* Configure Gyroscope and Accelerometer */ |
BaserK | 0:954f15bd95f1 | 176 | // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz |
BaserK | 0:954f15bd95f1 | 177 | // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate) |
BaserK | 0:954f15bd95f1 | 178 | writeByte(MPU6050_ADDRESS, CONFIG, 0x03); |
BaserK | 0:954f15bd95f1 | 179 | |
BaserK | 0:954f15bd95f1 | 180 | /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */ |
BaserK | 0:954f15bd95f1 | 181 | // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above) |
BaserK | 0:954f15bd95f1 | 182 | writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); |
BaserK | 0:954f15bd95f1 | 183 | |
BaserK | 0:954f15bd95f1 | 184 | /* Accelerometer configuration */ |
BaserK | 0:954f15bd95f1 | 185 | uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); |
BaserK | 0:954f15bd95f1 | 186 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] |
BaserK | 0:954f15bd95f1 | 187 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0x18); // Clear AFS bits [4:3] |
BaserK | 0:954f15bd95f1 | 188 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp | Ascale<<3); // Set full scale range |
BaserK | 0:954f15bd95f1 | 189 | |
BaserK | 0:954f15bd95f1 | 190 | /* Gyroscope configuration */ |
BaserK | 0:954f15bd95f1 | 191 | temp = readByte(MPU6050_ADDRESS, GYRO_CONFIG); |
BaserK | 0:954f15bd95f1 | 192 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] |
BaserK | 0:954f15bd95f1 | 193 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18); // Clear FS bits [4:3] |
BaserK | 0:954f15bd95f1 | 194 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3); // Set full scale range |
BaserK | 0:954f15bd95f1 | 195 | } |
BaserK | 0:954f15bd95f1 | 196 | |
BaserK | 0:954f15bd95f1 | 197 | // Resets the device |
BaserK | 0:954f15bd95f1 | 198 | void MPU6050::reset() |
BaserK | 0:954f15bd95f1 | 199 | { |
BaserK | 0:954f15bd95f1 | 200 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // set bit7 to reset the device |
BaserK | 0:954f15bd95f1 | 201 | wait_ms(100); // wait 100 ms to stabilize |
BaserK | 0:954f15bd95f1 | 202 | } |
BaserK | 0:954f15bd95f1 | 203 | |
BaserK | 0:954f15bd95f1 | 204 | void MPU6050::readAccelData(int16_t* dest) |
BaserK | 0:954f15bd95f1 | 205 | { |
BaserK | 0:954f15bd95f1 | 206 | uint8_t rawData[6]; // x,y,z acc data |
BaserK | 0:954f15bd95f1 | 207 | readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // read six raw data registers sequentially and write them into data array |
BaserK | 0:954f15bd95f1 | 208 | |
BaserK | 0:954f15bd95f1 | 209 | /* Turn the MSB LSB into signed 16-bit value */ |
BaserK | 0:954f15bd95f1 | 210 | dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // ACCEL_XOUT |
BaserK | 0:954f15bd95f1 | 211 | dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // ACCEL_YOUT |
BaserK | 0:954f15bd95f1 | 212 | dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // ACCEL_ZOUT |
BaserK | 0:954f15bd95f1 | 213 | } |
BaserK | 0:954f15bd95f1 | 214 | |
BaserK | 0:954f15bd95f1 | 215 | void MPU6050::readGyroData(int16_t* dest) |
BaserK | 0:954f15bd95f1 | 216 | { |
BaserK | 0:954f15bd95f1 | 217 | uint8_t rawData[6]; // x,y,z gyro data |
BaserK | 0:954f15bd95f1 | 218 | 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 | 219 | |
BaserK | 0:954f15bd95f1 | 220 | /* Turn the MSB LSB into signed 16-bit value */ |
BaserK | 0:954f15bd95f1 | 221 | dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // GYRO_XOUT |
BaserK | 0:954f15bd95f1 | 222 | dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // GYRO_YOUT |
BaserK | 0:954f15bd95f1 | 223 | dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // GYRO_ZOUT |
BaserK | 0:954f15bd95f1 | 224 | } |
BaserK | 0:954f15bd95f1 | 225 | |
BaserK | 0:954f15bd95f1 | 226 | int16_t MPU6050::readTempData() |
BaserK | 0:954f15bd95f1 | 227 | { |
BaserK | 0:954f15bd95f1 | 228 | uint8_t rawData[2]; // temperature data |
BaserK | 0:954f15bd95f1 | 229 | 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 | 230 | return (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // turn the MSB LSB into signed 16-bit value |
BaserK | 0:954f15bd95f1 | 231 | } |
BaserK | 0:954f15bd95f1 | 232 | |
BaserK | 0:954f15bd95f1 | 233 | /* Function which accumulates gyro and accelerometer data after device initialization. |
BaserK | 0:954f15bd95f1 | 234 | It calculates the average of the at-rest readings and |
BaserK | 0:954f15bd95f1 | 235 | then loads the resulting offsets into accelerometer and gyro bias registers. */ |
BaserK | 0:954f15bd95f1 | 236 | /* |
BaserK | 0:954f15bd95f1 | 237 | IMPORTANT NOTE: In this function; |
BaserK | 0:954f15bd95f1 | 238 | Resulting accel offsets are NOT pushed to the accel bias registers. accelBias[i] offsets are used in the main program. |
BaserK | 0:954f15bd95f1 | 239 | Resulting gyro offsets are pushed to the gyro bias registers. gyroBias[i] offsets are NOT used in the main program. |
BaserK | 0:954f15bd95f1 | 240 | Resulting data seems satisfactory. |
BaserK | 0:954f15bd95f1 | 241 | */ |
BaserK | 0:954f15bd95f1 | 242 | // dest1: accelBias dest2: gyroBias |
BaserK | 0:954f15bd95f1 | 243 | void MPU6050::calibrate(float* dest1, float* dest2) |
BaserK | 0:954f15bd95f1 | 244 | { |
BaserK | 0:954f15bd95f1 | 245 | uint8_t data[12]; // data array to hold acc and gyro x,y,z data |
BaserK | 0:954f15bd95f1 | 246 | uint16_t fifo_count, packet_count, count; |
BaserK | 0:954f15bd95f1 | 247 | int32_t accel_bias[3] = {0,0,0}; |
BaserK | 0:954f15bd95f1 | 248 | int32_t gyro_bias[3] = {0,0,0}; |
BaserK | 0:954f15bd95f1 | 249 | float aRes = 2.0/32768.0; |
BaserK | 0:954f15bd95f1 | 250 | float gRes = 250.0/32768.0; |
BaserK | 0:954f15bd95f1 | 251 | uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g |
BaserK | 0:954f15bd95f1 | 252 | //uint16_t gyrosensitivity = 131; // = 1/gRes = 131 LSB/dps |
BaserK | 0:954f15bd95f1 | 253 | |
BaserK | 0:954f15bd95f1 | 254 | reset(); // Reset device |
BaserK | 0:954f15bd95f1 | 255 | |
BaserK | 0:954f15bd95f1 | 256 | /* Get stable time source */ |
BaserK | 0:954f15bd95f1 | 257 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // PLL with X axis gyroscope reference is used to improve stability |
BaserK | 0:954f15bd95f1 | 258 | writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); // Disable accel only low power mode |
BaserK | 0:954f15bd95f1 | 259 | wait(0.2); |
BaserK | 0:954f15bd95f1 | 260 | |
BaserK | 0:954f15bd95f1 | 261 | /* Configure device for bias calculation */ |
BaserK | 0:954f15bd95f1 | 262 | writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts |
BaserK | 0:954f15bd95f1 | 263 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO |
BaserK | 0:954f15bd95f1 | 264 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source |
BaserK | 0:954f15bd95f1 | 265 | writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master |
BaserK | 0:954f15bd95f1 | 266 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes |
BaserK | 0:954f15bd95f1 | 267 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x04); // Reset FIFO |
BaserK | 0:954f15bd95f1 | 268 | wait(0.015); |
BaserK | 0:954f15bd95f1 | 269 | |
BaserK | 0:954f15bd95f1 | 270 | /* Configure accel and gyro for bias calculation */ |
BaserK | 0:954f15bd95f1 | 271 | writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz |
BaserK | 0:954f15bd95f1 | 272 | writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz |
BaserK | 0:954f15bd95f1 | 273 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity |
BaserK | 0:954f15bd95f1 | 274 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity |
BaserK | 0:954f15bd95f1 | 275 | |
BaserK | 0:954f15bd95f1 | 276 | /* Configure FIFO to capture accelerometer and gyro data for bias calculation */ |
BaserK | 0:954f15bd95f1 | 277 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO |
BaserK | 0:954f15bd95f1 | 278 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable accelerometer and gyro for FIFO (max size 1024 bytes in MPU-6050) |
BaserK | 0:954f15bd95f1 | 279 | wait(0.08); // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds. |
BaserK | 0:954f15bd95f1 | 280 | // 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 | 281 | |
BaserK | 0:954f15bd95f1 | 282 | /* At end of sample accumulation, turn off FIFO sensor read */ |
BaserK | 0:954f15bd95f1 | 283 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO |
BaserK | 0:954f15bd95f1 | 284 | readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // Read FIFO sample count |
BaserK | 0:954f15bd95f1 | 285 | fifo_count = ((uint16_t)data[0] << 8) | data[1]; |
BaserK | 0:954f15bd95f1 | 286 | 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 | 287 | |
BaserK | 0:954f15bd95f1 | 288 | for(count=0; count<packet_count; count++) |
BaserK | 0:954f15bd95f1 | 289 | { |
BaserK | 0:954f15bd95f1 | 290 | int16_t accel_temp[3]={0,0,0}; |
BaserK | 0:954f15bd95f1 | 291 | int16_t gyro_temp[3]={0,0,0}; |
BaserK | 0:954f15bd95f1 | 292 | readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging |
BaserK | 0:954f15bd95f1 | 293 | |
BaserK | 0:954f15bd95f1 | 294 | /* Form signed 16-bit integer for each sample in FIFO */ |
BaserK | 0:954f15bd95f1 | 295 | accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; |
BaserK | 0:954f15bd95f1 | 296 | accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; |
BaserK | 0:954f15bd95f1 | 297 | accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; |
BaserK | 0:954f15bd95f1 | 298 | gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; |
BaserK | 0:954f15bd95f1 | 299 | gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; |
BaserK | 0:954f15bd95f1 | 300 | gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; |
BaserK | 0:954f15bd95f1 | 301 | |
BaserK | 0:954f15bd95f1 | 302 | /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */ |
BaserK | 0:954f15bd95f1 | 303 | accel_bias[0] += (int32_t) accel_temp[0]; |
BaserK | 0:954f15bd95f1 | 304 | accel_bias[1] += (int32_t) accel_temp[1]; |
BaserK | 0:954f15bd95f1 | 305 | accel_bias[2] += (int32_t) accel_temp[2]; |
BaserK | 0:954f15bd95f1 | 306 | gyro_bias[0] += (int32_t) gyro_temp[0]; |
BaserK | 0:954f15bd95f1 | 307 | gyro_bias[1] += (int32_t) gyro_temp[1]; |
BaserK | 0:954f15bd95f1 | 308 | gyro_bias[2] += (int32_t) gyro_temp[2]; |
BaserK | 0:954f15bd95f1 | 309 | } |
BaserK | 0:954f15bd95f1 | 310 | |
BaserK | 0:954f15bd95f1 | 311 | /* Normalize sums to get average count biases */ |
BaserK | 0:954f15bd95f1 | 312 | accel_bias[0] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 313 | accel_bias[1] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 314 | accel_bias[2] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 315 | gyro_bias[0] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 316 | gyro_bias[1] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 317 | gyro_bias[2] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 318 | |
BaserK | 0:954f15bd95f1 | 319 | /* Remove gravity from the z-axis accelerometer bias calculation */ |
BaserK | 0:954f15bd95f1 | 320 | if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;} |
BaserK | 0:954f15bd95f1 | 321 | else {accel_bias[2] += (int32_t) accelsensitivity;} |
BaserK | 0:954f15bd95f1 | 322 | |
BaserK | 0:954f15bd95f1 | 323 | /* Output scaled accelerometer biases for manual subtraction in the main program */ |
BaserK | 0:954f15bd95f1 | 324 | dest1[0] = accel_bias[0]*aRes; |
BaserK | 0:954f15bd95f1 | 325 | dest1[1] = accel_bias[1]*aRes; |
BaserK | 0:954f15bd95f1 | 326 | dest1[2] = accel_bias[2]*aRes; |
BaserK | 0:954f15bd95f1 | 327 | |
BaserK | 0:954f15bd95f1 | 328 | /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */ |
BaserK | 0:954f15bd95f1 | 329 | 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 | 330 | data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases |
BaserK | 0:954f15bd95f1 | 331 | data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; |
BaserK | 0:954f15bd95f1 | 332 | data[3] = (-gyro_bias[1]/4) & 0xFF; |
BaserK | 0:954f15bd95f1 | 333 | data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; |
BaserK | 0:954f15bd95f1 | 334 | data[5] = (-gyro_bias[2]/4) & 0xFF; |
BaserK | 0:954f15bd95f1 | 335 | |
BaserK | 0:954f15bd95f1 | 336 | /* Push gyro biases to hardware registers */ |
BaserK | 0:954f15bd95f1 | 337 | writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); |
BaserK | 0:954f15bd95f1 | 338 | writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]); |
BaserK | 0:954f15bd95f1 | 339 | writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]); |
BaserK | 0:954f15bd95f1 | 340 | writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]); |
BaserK | 0:954f15bd95f1 | 341 | writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]); |
BaserK | 0:954f15bd95f1 | 342 | writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]); |
BaserK | 0:954f15bd95f1 | 343 | |
BaserK | 0:954f15bd95f1 | 344 | /* Construct gyro bias in deg/s for later manual subtraction */ |
BaserK | 0:954f15bd95f1 | 345 | dest2[0] = gyro_bias[0]*gRes; |
BaserK | 0:954f15bd95f1 | 346 | dest2[1] = gyro_bias[1]*gRes; |
BaserK | 0:954f15bd95f1 | 347 | dest2[2] = gyro_bias[2]*gRes; |
BaserK | 0:954f15bd95f1 | 348 | } |
BaserK | 2:3e0dfce73a58 | 349 | |
BaserK | 2:3e0dfce73a58 | 350 | void MPU6050::complementaryFilter(float* pitch, float* roll) |
BaserK | 2:3e0dfce73a58 | 351 | { |
BaserK | 2:3e0dfce73a58 | 352 | /* Get actual acc value */ |
BaserK | 2:3e0dfce73a58 | 353 | readAccelData(accelData); |
BaserK | 2:3e0dfce73a58 | 354 | getAres(); |
BaserK | 2:3e0dfce73a58 | 355 | ax = accelData[0]*aRes - accelBias[0]; |
BaserK | 2:3e0dfce73a58 | 356 | ay = accelData[1]*aRes - accelBias[1]; |
BaserK | 2:3e0dfce73a58 | 357 | az = accelData[2]*aRes - accelBias[2]; |
BaserK | 2:3e0dfce73a58 | 358 | |
BaserK | 2:3e0dfce73a58 | 359 | /* Get actual gyro value */ |
BaserK | 2:3e0dfce73a58 | 360 | readGyroData(gyroData); |
BaserK | 2:3e0dfce73a58 | 361 | getGres(); |
BaserK | 2:3e0dfce73a58 | 362 | gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i] |
BaserK | 2:3e0dfce73a58 | 363 | gy = gyroData[1]*gRes; // - gyroBias[1]; |
BaserK | 2:3e0dfce73a58 | 364 | gz = gyroData[2]*gRes; // - gyroBias[2]; |
BaserK | 2:3e0dfce73a58 | 365 | |
BaserK | 2:3e0dfce73a58 | 366 | float pitchAcc, rollAcc; |
BaserK | 2:3e0dfce73a58 | 367 | |
BaserK | 2:3e0dfce73a58 | 368 | /* Integrate the gyro data(deg/s) over time to get angle */ |
BaserK | 2:3e0dfce73a58 | 369 | *pitch += gx * dt; // Angle around the X-axis |
BaserK | 2:3e0dfce73a58 | 370 | *roll -= gy * dt; // Angle around the Y-axis |
BaserK | 2:3e0dfce73a58 | 371 | |
BaserK | 2:3e0dfce73a58 | 372 | /* Turning around the X-axis results in a vector on the Y-axis |
BaserK | 2:3e0dfce73a58 | 373 | whereas turning around the Y-axis results in a vector on the X-axis. */ |
BaserK | 2:3e0dfce73a58 | 374 | pitchAcc = atan2f(accelData[1], accelData[2])*180/PI; |
BaserK | 2:3e0dfce73a58 | 375 | rollAcc = atan2f(accelData[0], accelData[2])*180/PI; |
BaserK | 2:3e0dfce73a58 | 376 | |
BaserK | 2:3e0dfce73a58 | 377 | /* Apply Complementary Filter */ |
BaserK | 2:3e0dfce73a58 | 378 | *pitch = *pitch * 0.98 + pitchAcc * 0.02; |
BaserK | 2:3e0dfce73a58 | 379 | *roll = *roll * 0.98 + rollAcc * 0.02; |
BaserK | 2:3e0dfce73a58 | 380 | } |