a
Fork of MPU6050 by
MPU6050.cpp@7:86f32d1ded68, 2018-09-07 (annotated)
- Committer:
- tktats_892476
- Date:
- Fri Sep 07 09:22:23 2018 +0000
- Revision:
- 7:86f32d1ded68
- Parent:
- 6:5b90f2b5e6d9
180907
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 | 6:5b90f2b5e6d9 | 37 | static 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 | |
tktats_892476 | 7:86f32d1ded68 | 66 | float rawx=0;//16_6_12追加gxの積算のみの書く度,加速度センサを使わない |
tktats_892476 | 7:86f32d1ded68 | 67 | |
BaserK | 0:954f15bd95f1 | 68 | // Specify sensor full scale range |
BaserK | 0:954f15bd95f1 | 69 | int Ascale = AFS_2G; |
BaserK | 0:954f15bd95f1 | 70 | int Gscale = GFS_250DPS; |
BaserK | 0:954f15bd95f1 | 71 | |
BaserK | 0:954f15bd95f1 | 72 | // Scale resolutions per LSB for the sensors |
BaserK | 0:954f15bd95f1 | 73 | float aRes, gRes; |
BaserK | 0:954f15bd95f1 | 74 | |
BaserK | 0:954f15bd95f1 | 75 | // Calculates Acc resolution |
BaserK | 0:954f15bd95f1 | 76 | void MPU6050::getAres() |
BaserK | 0:954f15bd95f1 | 77 | { |
BaserK | 0:954f15bd95f1 | 78 | switch(Ascale) |
BaserK | 0:954f15bd95f1 | 79 | { |
BaserK | 0:954f15bd95f1 | 80 | case AFS_2G: |
BaserK | 0:954f15bd95f1 | 81 | aRes = 2.0/32768.0; |
BaserK | 0:954f15bd95f1 | 82 | break; |
BaserK | 0:954f15bd95f1 | 83 | case AFS_4G: |
BaserK | 0:954f15bd95f1 | 84 | aRes = 4.0/32768.0; |
BaserK | 0:954f15bd95f1 | 85 | break; |
BaserK | 0:954f15bd95f1 | 86 | case AFS_8G: |
BaserK | 0:954f15bd95f1 | 87 | aRes = 8.0/32768.0; |
BaserK | 0:954f15bd95f1 | 88 | break; |
BaserK | 0:954f15bd95f1 | 89 | case AFS_16G: |
BaserK | 0:954f15bd95f1 | 90 | aRes = 16.0/32768.0; |
BaserK | 0:954f15bd95f1 | 91 | break; |
BaserK | 0:954f15bd95f1 | 92 | } |
BaserK | 0:954f15bd95f1 | 93 | } |
BaserK | 0:954f15bd95f1 | 94 | |
BaserK | 0:954f15bd95f1 | 95 | // Calculates Gyro resolution |
BaserK | 0:954f15bd95f1 | 96 | void MPU6050::getGres() |
BaserK | 0:954f15bd95f1 | 97 | { |
BaserK | 0:954f15bd95f1 | 98 | switch(Gscale) |
BaserK | 0:954f15bd95f1 | 99 | { |
BaserK | 0:954f15bd95f1 | 100 | case GFS_250DPS: |
BaserK | 0:954f15bd95f1 | 101 | gRes = 250.0/32768.0; |
BaserK | 0:954f15bd95f1 | 102 | break; |
BaserK | 0:954f15bd95f1 | 103 | case GFS_500DPS: |
BaserK | 0:954f15bd95f1 | 104 | gRes = 500.0/32768.0; |
BaserK | 0:954f15bd95f1 | 105 | break; |
BaserK | 0:954f15bd95f1 | 106 | case GFS_1000DPS: |
BaserK | 0:954f15bd95f1 | 107 | gRes = 1000.0/32768.0; |
BaserK | 0:954f15bd95f1 | 108 | break; |
BaserK | 0:954f15bd95f1 | 109 | case GFS_2000DPS: |
BaserK | 0:954f15bd95f1 | 110 | gRes = 2000.0/32768.0; |
BaserK | 0:954f15bd95f1 | 111 | break; |
BaserK | 0:954f15bd95f1 | 112 | } |
BaserK | 0:954f15bd95f1 | 113 | } |
BaserK | 0:954f15bd95f1 | 114 | |
BaserK | 0:954f15bd95f1 | 115 | void MPU6050::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) |
BaserK | 0:954f15bd95f1 | 116 | { |
BaserK | 0:954f15bd95f1 | 117 | char data_write[2]; |
BaserK | 0:954f15bd95f1 | 118 | data_write[0]=subAddress; // I2C sends MSB first. Namely >>|subAddress|>>|data| |
BaserK | 0:954f15bd95f1 | 119 | data_write[1]=data; |
BaserK | 0:954f15bd95f1 | 120 | i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false); |
BaserK | 0:954f15bd95f1 | 121 | } |
BaserK | 0:954f15bd95f1 | 122 | |
BaserK | 0:954f15bd95f1 | 123 | char MPU6050::readByte(uint8_t address, uint8_t subAddress) |
BaserK | 0:954f15bd95f1 | 124 | { |
BaserK | 0:954f15bd95f1 | 125 | char data_read[1]; // will store the register data |
BaserK | 0:954f15bd95f1 | 126 | char data_write[1]; |
BaserK | 0:954f15bd95f1 | 127 | data_write[0]=subAddress; |
BaserK | 0:954f15bd95f1 | 128 | i2c.write(address,data_write,1,1); // have not stopped yet |
BaserK | 0:954f15bd95f1 | 129 | i2c.read(address,data_read,1,0); // read the data and stop |
BaserK | 0:954f15bd95f1 | 130 | return data_read[0]; |
BaserK | 0:954f15bd95f1 | 131 | } |
BaserK | 0:954f15bd95f1 | 132 | |
BaserK | 0:954f15bd95f1 | 133 | void MPU6050::readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest) |
BaserK | 0:954f15bd95f1 | 134 | { |
BaserK | 0:954f15bd95f1 | 135 | char data[14],data_write[1]; |
BaserK | 0:954f15bd95f1 | 136 | data_write[0]=subAddress; |
BaserK | 0:954f15bd95f1 | 137 | i2c.write(address,data_write,1,1); |
BaserK | 0:954f15bd95f1 | 138 | i2c.read(address,data,byteNum,0); |
BaserK | 0:954f15bd95f1 | 139 | for(int i=0;i<byteNum;i++) // equate the addresses |
BaserK | 0:954f15bd95f1 | 140 | dest[i]=data[i]; |
BaserK | 0:954f15bd95f1 | 141 | } |
BaserK | 0:954f15bd95f1 | 142 | |
BaserK | 0:954f15bd95f1 | 143 | // Communication test: WHO_AM_I register reading |
BaserK | 0:954f15bd95f1 | 144 | void MPU6050::whoAmI() |
BaserK | 0:954f15bd95f1 | 145 | { |
BaserK | 0:954f15bd95f1 | 146 | uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Should return 0x68 |
BaserK | 2:3e0dfce73a58 | 147 | pc.printf("I AM 0x%x \r\n",whoAmI); |
BaserK | 0:954f15bd95f1 | 148 | |
BaserK | 0:954f15bd95f1 | 149 | if(whoAmI==0x68) |
BaserK | 0:954f15bd95f1 | 150 | { |
BaserK | 2:3e0dfce73a58 | 151 | pc.printf("MPU6050 is online... \r\n"); |
BaserK | 0:954f15bd95f1 | 152 | led2=1; |
BaserK | 0:954f15bd95f1 | 153 | } |
BaserK | 0:954f15bd95f1 | 154 | else |
BaserK | 0:954f15bd95f1 | 155 | { |
BaserK | 2:3e0dfce73a58 | 156 | pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n"); |
BaserK | 0:954f15bd95f1 | 157 | toggler1.attach(&toggle_led1,0.1); // toggles led1 every 100 ms |
BaserK | 0:954f15bd95f1 | 158 | } |
BaserK | 0:954f15bd95f1 | 159 | } |
BaserK | 0:954f15bd95f1 | 160 | |
BaserK | 0:954f15bd95f1 | 161 | // Initializes MPU6050 with the following config: |
BaserK | 0:954f15bd95f1 | 162 | // PLL with X axis gyroscope reference |
BaserK | 0:954f15bd95f1 | 163 | // Sample rate: 200Hz for gyro and acc |
BaserK | 0:954f15bd95f1 | 164 | // Interrupts are disabled |
BaserK | 0:954f15bd95f1 | 165 | void MPU6050::init() |
BaserK | 5:5bff0edcdff8 | 166 | { |
BaserK | 5:5bff0edcdff8 | 167 | i2c.frequency(400000); // fast i2c: 400 kHz |
BaserK | 5:5bff0edcdff8 | 168 | |
BaserK | 0:954f15bd95f1 | 169 | /* Wake up the device */ |
BaserK | 0:954f15bd95f1 | 170 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // wake up the device by clearing the sleep bit (bit6) |
BaserK | 0:954f15bd95f1 | 171 | wait_ms(100); // wait 100 ms to stabilize |
BaserK | 0:954f15bd95f1 | 172 | |
BaserK | 0:954f15bd95f1 | 173 | /* Get stable time source */ |
BaserK | 0:954f15bd95f1 | 174 | // PLL with X axis gyroscope reference is used to improve stability |
BaserK | 0:954f15bd95f1 | 175 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); |
BaserK | 0:954f15bd95f1 | 176 | |
BaserK | 0:954f15bd95f1 | 177 | /* Configure Gyroscope and Accelerometer */ |
BaserK | 0:954f15bd95f1 | 178 | // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz |
BaserK | 0:954f15bd95f1 | 179 | // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate) |
BaserK | 0:954f15bd95f1 | 180 | writeByte(MPU6050_ADDRESS, CONFIG, 0x03); |
BaserK | 0:954f15bd95f1 | 181 | |
BaserK | 0:954f15bd95f1 | 182 | /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */ |
BaserK | 0:954f15bd95f1 | 183 | // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above) |
BaserK | 0:954f15bd95f1 | 184 | writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); |
BaserK | 0:954f15bd95f1 | 185 | |
BaserK | 0:954f15bd95f1 | 186 | /* Accelerometer configuration */ |
BaserK | 0:954f15bd95f1 | 187 | uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); |
BaserK | 0:954f15bd95f1 | 188 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] |
BaserK | 0:954f15bd95f1 | 189 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0x18); // Clear AFS bits [4:3] |
BaserK | 0:954f15bd95f1 | 190 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp | Ascale<<3); // Set full scale range |
BaserK | 0:954f15bd95f1 | 191 | |
BaserK | 0:954f15bd95f1 | 192 | /* Gyroscope configuration */ |
BaserK | 0:954f15bd95f1 | 193 | temp = readByte(MPU6050_ADDRESS, GYRO_CONFIG); |
BaserK | 0:954f15bd95f1 | 194 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] |
BaserK | 0:954f15bd95f1 | 195 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18); // Clear FS bits [4:3] |
BaserK | 0:954f15bd95f1 | 196 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3); // Set full scale range |
BaserK | 0:954f15bd95f1 | 197 | } |
BaserK | 0:954f15bd95f1 | 198 | |
BaserK | 0:954f15bd95f1 | 199 | // Resets the device |
BaserK | 0:954f15bd95f1 | 200 | void MPU6050::reset() |
BaserK | 0:954f15bd95f1 | 201 | { |
BaserK | 0:954f15bd95f1 | 202 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // set bit7 to reset the device |
BaserK | 0:954f15bd95f1 | 203 | wait_ms(100); // wait 100 ms to stabilize |
BaserK | 0:954f15bd95f1 | 204 | } |
BaserK | 0:954f15bd95f1 | 205 | |
BaserK | 0:954f15bd95f1 | 206 | void MPU6050::readAccelData(int16_t* dest) |
BaserK | 0:954f15bd95f1 | 207 | { |
BaserK | 0:954f15bd95f1 | 208 | uint8_t rawData[6]; // x,y,z acc data |
BaserK | 0:954f15bd95f1 | 209 | readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // read six raw data registers sequentially and write them into data array |
BaserK | 0:954f15bd95f1 | 210 | |
BaserK | 0:954f15bd95f1 | 211 | /* Turn the MSB LSB into signed 16-bit value */ |
BaserK | 0:954f15bd95f1 | 212 | dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // ACCEL_XOUT |
BaserK | 0:954f15bd95f1 | 213 | dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // ACCEL_YOUT |
BaserK | 0:954f15bd95f1 | 214 | dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // ACCEL_ZOUT |
BaserK | 0:954f15bd95f1 | 215 | } |
BaserK | 0:954f15bd95f1 | 216 | |
BaserK | 0:954f15bd95f1 | 217 | void MPU6050::readGyroData(int16_t* dest) |
BaserK | 0:954f15bd95f1 | 218 | { |
BaserK | 0:954f15bd95f1 | 219 | uint8_t rawData[6]; // x,y,z gyro data |
BaserK | 0:954f15bd95f1 | 220 | 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 | 221 | |
BaserK | 0:954f15bd95f1 | 222 | /* Turn the MSB LSB into signed 16-bit value */ |
BaserK | 0:954f15bd95f1 | 223 | dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // GYRO_XOUT |
BaserK | 0:954f15bd95f1 | 224 | dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // GYRO_YOUT |
BaserK | 0:954f15bd95f1 | 225 | dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // GYRO_ZOUT |
BaserK | 0:954f15bd95f1 | 226 | } |
BaserK | 0:954f15bd95f1 | 227 | |
BaserK | 0:954f15bd95f1 | 228 | int16_t MPU6050::readTempData() |
BaserK | 0:954f15bd95f1 | 229 | { |
BaserK | 0:954f15bd95f1 | 230 | uint8_t rawData[2]; // temperature data |
BaserK | 0:954f15bd95f1 | 231 | 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 | 232 | return (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // turn the MSB LSB into signed 16-bit value |
BaserK | 0:954f15bd95f1 | 233 | } |
BaserK | 0:954f15bd95f1 | 234 | |
BaserK | 0:954f15bd95f1 | 235 | /* Function which accumulates gyro and accelerometer data after device initialization. |
BaserK | 0:954f15bd95f1 | 236 | It calculates the average of the at-rest readings and |
BaserK | 0:954f15bd95f1 | 237 | then loads the resulting offsets into accelerometer and gyro bias registers. */ |
BaserK | 0:954f15bd95f1 | 238 | /* |
BaserK | 0:954f15bd95f1 | 239 | IMPORTANT NOTE: In this function; |
BaserK | 0:954f15bd95f1 | 240 | Resulting accel offsets are NOT pushed to the accel bias registers. accelBias[i] offsets are used in the main program. |
BaserK | 0:954f15bd95f1 | 241 | Resulting gyro offsets are pushed to the gyro bias registers. gyroBias[i] offsets are NOT used in the main program. |
BaserK | 0:954f15bd95f1 | 242 | Resulting data seems satisfactory. |
BaserK | 0:954f15bd95f1 | 243 | */ |
BaserK | 0:954f15bd95f1 | 244 | // dest1: accelBias dest2: gyroBias |
BaserK | 0:954f15bd95f1 | 245 | void MPU6050::calibrate(float* dest1, float* dest2) |
BaserK | 0:954f15bd95f1 | 246 | { |
BaserK | 0:954f15bd95f1 | 247 | uint8_t data[12]; // data array to hold acc and gyro x,y,z data |
BaserK | 0:954f15bd95f1 | 248 | uint16_t fifo_count, packet_count, count; |
BaserK | 0:954f15bd95f1 | 249 | int32_t accel_bias[3] = {0,0,0}; |
BaserK | 0:954f15bd95f1 | 250 | int32_t gyro_bias[3] = {0,0,0}; |
BaserK | 0:954f15bd95f1 | 251 | float aRes = 2.0/32768.0; |
BaserK | 0:954f15bd95f1 | 252 | float gRes = 250.0/32768.0; |
BaserK | 0:954f15bd95f1 | 253 | uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g |
BaserK | 0:954f15bd95f1 | 254 | //uint16_t gyrosensitivity = 131; // = 1/gRes = 131 LSB/dps |
BaserK | 0:954f15bd95f1 | 255 | |
BaserK | 0:954f15bd95f1 | 256 | reset(); // Reset device |
BaserK | 0:954f15bd95f1 | 257 | |
BaserK | 0:954f15bd95f1 | 258 | /* Get stable time source */ |
BaserK | 0:954f15bd95f1 | 259 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // PLL with X axis gyroscope reference is used to improve stability |
BaserK | 0:954f15bd95f1 | 260 | writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); // Disable accel only low power mode |
BaserK | 0:954f15bd95f1 | 261 | wait(0.2); |
BaserK | 0:954f15bd95f1 | 262 | |
BaserK | 0:954f15bd95f1 | 263 | /* Configure device for bias calculation */ |
BaserK | 0:954f15bd95f1 | 264 | writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts |
BaserK | 0:954f15bd95f1 | 265 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO |
BaserK | 0:954f15bd95f1 | 266 | writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source |
BaserK | 0:954f15bd95f1 | 267 | writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master |
BaserK | 0:954f15bd95f1 | 268 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes |
BaserK | 0:954f15bd95f1 | 269 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x04); // Reset FIFO |
BaserK | 0:954f15bd95f1 | 270 | wait(0.015); |
BaserK | 0:954f15bd95f1 | 271 | |
BaserK | 0:954f15bd95f1 | 272 | /* Configure accel and gyro for bias calculation */ |
BaserK | 0:954f15bd95f1 | 273 | writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz |
BaserK | 0:954f15bd95f1 | 274 | writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz |
BaserK | 0:954f15bd95f1 | 275 | writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity |
BaserK | 0:954f15bd95f1 | 276 | writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity |
BaserK | 0:954f15bd95f1 | 277 | |
BaserK | 0:954f15bd95f1 | 278 | /* Configure FIFO to capture accelerometer and gyro data for bias calculation */ |
BaserK | 0:954f15bd95f1 | 279 | writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO |
BaserK | 0:954f15bd95f1 | 280 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable accelerometer and gyro for FIFO (max size 1024 bytes in MPU-6050) |
BaserK | 0:954f15bd95f1 | 281 | wait(0.08); // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds. |
BaserK | 0:954f15bd95f1 | 282 | // 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 | 283 | |
BaserK | 0:954f15bd95f1 | 284 | /* At end of sample accumulation, turn off FIFO sensor read */ |
BaserK | 0:954f15bd95f1 | 285 | writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO |
BaserK | 0:954f15bd95f1 | 286 | readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // Read FIFO sample count |
BaserK | 0:954f15bd95f1 | 287 | fifo_count = ((uint16_t)data[0] << 8) | data[1]; |
BaserK | 0:954f15bd95f1 | 288 | 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 | 289 | |
BaserK | 0:954f15bd95f1 | 290 | for(count=0; count<packet_count; count++) |
BaserK | 0:954f15bd95f1 | 291 | { |
BaserK | 0:954f15bd95f1 | 292 | int16_t accel_temp[3]={0,0,0}; |
BaserK | 0:954f15bd95f1 | 293 | int16_t gyro_temp[3]={0,0,0}; |
BaserK | 0:954f15bd95f1 | 294 | readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging |
BaserK | 0:954f15bd95f1 | 295 | |
BaserK | 0:954f15bd95f1 | 296 | /* Form signed 16-bit integer for each sample in FIFO */ |
BaserK | 0:954f15bd95f1 | 297 | accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; |
BaserK | 0:954f15bd95f1 | 298 | accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; |
BaserK | 0:954f15bd95f1 | 299 | accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; |
BaserK | 0:954f15bd95f1 | 300 | gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; |
BaserK | 0:954f15bd95f1 | 301 | gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; |
BaserK | 0:954f15bd95f1 | 302 | gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; |
BaserK | 0:954f15bd95f1 | 303 | |
BaserK | 0:954f15bd95f1 | 304 | /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */ |
BaserK | 0:954f15bd95f1 | 305 | accel_bias[0] += (int32_t) accel_temp[0]; |
BaserK | 0:954f15bd95f1 | 306 | accel_bias[1] += (int32_t) accel_temp[1]; |
BaserK | 0:954f15bd95f1 | 307 | accel_bias[2] += (int32_t) accel_temp[2]; |
BaserK | 0:954f15bd95f1 | 308 | gyro_bias[0] += (int32_t) gyro_temp[0]; |
BaserK | 0:954f15bd95f1 | 309 | gyro_bias[1] += (int32_t) gyro_temp[1]; |
BaserK | 0:954f15bd95f1 | 310 | gyro_bias[2] += (int32_t) gyro_temp[2]; |
BaserK | 0:954f15bd95f1 | 311 | } |
BaserK | 0:954f15bd95f1 | 312 | |
BaserK | 0:954f15bd95f1 | 313 | /* Normalize sums to get average count biases */ |
BaserK | 0:954f15bd95f1 | 314 | accel_bias[0] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 315 | accel_bias[1] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 316 | accel_bias[2] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 317 | gyro_bias[0] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 318 | gyro_bias[1] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 319 | gyro_bias[2] /= (int32_t) packet_count; |
BaserK | 0:954f15bd95f1 | 320 | |
BaserK | 0:954f15bd95f1 | 321 | /* Remove gravity from the z-axis accelerometer bias calculation */ |
BaserK | 0:954f15bd95f1 | 322 | if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;} |
BaserK | 0:954f15bd95f1 | 323 | else {accel_bias[2] += (int32_t) accelsensitivity;} |
BaserK | 0:954f15bd95f1 | 324 | |
BaserK | 0:954f15bd95f1 | 325 | /* Output scaled accelerometer biases for manual subtraction in the main program */ |
BaserK | 0:954f15bd95f1 | 326 | dest1[0] = accel_bias[0]*aRes; |
BaserK | 0:954f15bd95f1 | 327 | dest1[1] = accel_bias[1]*aRes; |
BaserK | 0:954f15bd95f1 | 328 | dest1[2] = accel_bias[2]*aRes; |
BaserK | 0:954f15bd95f1 | 329 | |
BaserK | 0:954f15bd95f1 | 330 | /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */ |
BaserK | 0:954f15bd95f1 | 331 | 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 | 332 | data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases |
BaserK | 0:954f15bd95f1 | 333 | data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; |
BaserK | 0:954f15bd95f1 | 334 | data[3] = (-gyro_bias[1]/4) & 0xFF; |
BaserK | 0:954f15bd95f1 | 335 | data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; |
BaserK | 0:954f15bd95f1 | 336 | data[5] = (-gyro_bias[2]/4) & 0xFF; |
BaserK | 0:954f15bd95f1 | 337 | |
BaserK | 0:954f15bd95f1 | 338 | /* Push gyro biases to hardware registers */ |
BaserK | 0:954f15bd95f1 | 339 | writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); |
BaserK | 0:954f15bd95f1 | 340 | writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]); |
BaserK | 0:954f15bd95f1 | 341 | writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]); |
BaserK | 0:954f15bd95f1 | 342 | writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]); |
BaserK | 0:954f15bd95f1 | 343 | writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]); |
BaserK | 0:954f15bd95f1 | 344 | writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]); |
BaserK | 0:954f15bd95f1 | 345 | |
BaserK | 0:954f15bd95f1 | 346 | /* Construct gyro bias in deg/s for later manual subtraction */ |
BaserK | 0:954f15bd95f1 | 347 | dest2[0] = gyro_bias[0]*gRes; |
BaserK | 0:954f15bd95f1 | 348 | dest2[1] = gyro_bias[1]*gRes; |
BaserK | 0:954f15bd95f1 | 349 | dest2[2] = gyro_bias[2]*gRes; |
BaserK | 0:954f15bd95f1 | 350 | } |
BaserK | 2:3e0dfce73a58 | 351 | |
BaserK | 2:3e0dfce73a58 | 352 | void MPU6050::complementaryFilter(float* pitch, float* roll) |
BaserK | 2:3e0dfce73a58 | 353 | { |
BaserK | 2:3e0dfce73a58 | 354 | /* Get actual acc value */ |
BaserK | 2:3e0dfce73a58 | 355 | readAccelData(accelData); |
BaserK | 2:3e0dfce73a58 | 356 | getAres(); |
BaserK | 2:3e0dfce73a58 | 357 | ax = accelData[0]*aRes - accelBias[0]; |
BaserK | 2:3e0dfce73a58 | 358 | ay = accelData[1]*aRes - accelBias[1]; |
BaserK | 2:3e0dfce73a58 | 359 | az = accelData[2]*aRes - accelBias[2]; |
BaserK | 2:3e0dfce73a58 | 360 | |
BaserK | 2:3e0dfce73a58 | 361 | /* Get actual gyro value */ |
BaserK | 2:3e0dfce73a58 | 362 | readGyroData(gyroData); |
BaserK | 2:3e0dfce73a58 | 363 | getGres(); |
BaserK | 2:3e0dfce73a58 | 364 | gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i] |
BaserK | 2:3e0dfce73a58 | 365 | gy = gyroData[1]*gRes; // - gyroBias[1]; |
BaserK | 2:3e0dfce73a58 | 366 | gz = gyroData[2]*gRes; // - gyroBias[2]; |
BaserK | 2:3e0dfce73a58 | 367 | |
BaserK | 2:3e0dfce73a58 | 368 | float pitchAcc, rollAcc; |
BaserK | 2:3e0dfce73a58 | 369 | |
BaserK | 2:3e0dfce73a58 | 370 | /* Integrate the gyro data(deg/s) over time to get angle */ |
BaserK | 2:3e0dfce73a58 | 371 | *pitch += gx * dt; // Angle around the X-axis |
BaserK | 2:3e0dfce73a58 | 372 | *roll -= gy * dt; // Angle around the Y-axis |
BaserK | 2:3e0dfce73a58 | 373 | |
BaserK | 2:3e0dfce73a58 | 374 | /* Turning around the X-axis results in a vector on the Y-axis |
BaserK | 2:3e0dfce73a58 | 375 | whereas turning around the Y-axis results in a vector on the X-axis. */ |
BaserK | 2:3e0dfce73a58 | 376 | pitchAcc = atan2f(accelData[1], accelData[2])*180/PI; |
BaserK | 2:3e0dfce73a58 | 377 | rollAcc = atan2f(accelData[0], accelData[2])*180/PI; |
BaserK | 2:3e0dfce73a58 | 378 | |
BaserK | 2:3e0dfce73a58 | 379 | /* Apply Complementary Filter */ |
BaserK | 2:3e0dfce73a58 | 380 | *pitch = *pitch * 0.98 + pitchAcc * 0.02; |
BaserK | 2:3e0dfce73a58 | 381 | *roll = *roll * 0.98 + rollAcc * 0.02; |
BaserK | 2:3e0dfce73a58 | 382 | } |