Sérgio Natan
teste
Embed:
(wiki syntax)
Show/hide line numbers
MPU6050.cpp
00001 /* MPU6050 Library 00002 * 00003 * @author: Baser Kandehir 00004 * @date: July 16, 2015 00005 * @license: MIT license 00006 * 00007 * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr 00008 * 00009 * Permission is hereby granted, free of charge, to any person obtaining a copy 00010 * of this software and associated documentation files (the "Software"), to deal 00011 * in the Software without restriction, including without limitation the rights 00012 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00013 * copies of the Software, and to permit persons to whom the Software is 00014 * furnished to do so, subject to the following conditions: 00015 * 00016 * The above copyright notice and this permission notice shall be included in 00017 * all copies or substantial portions of the Software. 00018 * 00019 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00020 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00021 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00022 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00023 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00024 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00025 * THE SOFTWARE. 00026 * 00027 */ 00028 00029 // Most of the code is adapted from Kris Winer's MPU6050 library 00030 00031 #include "MPU6050.h" 00032 00033 /* For LPC1768 board */ 00034 //I2C i2c(p9,p10); // setup i2c (SDA,SCL) 00035 00036 /* For NUCLEO-F411RE board */ 00037 //static I2C i2c(D14,D15); // setup i2c (SDA,SCL) 00038 00039 I2C i2c(PTE0,PTE1); // setup i2c (SDA,SCL) 00040 00041 /* Set initial input parameters */ 00042 00043 // Acc Full Scale Range +-2G 4G 8G 16G 00044 enum Ascale 00045 { 00046 AFS_2G=0, 00047 AFS_4G, 00048 AFS_8G, 00049 AFS_16G 00050 }; 00051 00052 // Gyro Full Scale Range +-250 500 1000 2000 Degrees per second 00053 enum Gscale 00054 { 00055 GFS_250DPS=0, 00056 GFS_500DPS, 00057 GFS_1000DPS, 00058 GFS_2000DPS 00059 }; 00060 00061 // Sensor datas 00062 float ax,ay,az; 00063 float gx,gy,gz; 00064 int16_t accelData[3],gyroData[3],tempData; 00065 float accelBias[3] = {0, 0, 0}; // Bias corrections for acc 00066 float gyroBias[3] = {0, 0, 0}; // Bias corrections for gyro 00067 00068 // Specify sensor full scale range 00069 int Ascale = AFS_2G; 00070 int Gscale = GFS_250DPS; 00071 00072 // Scale resolutions per LSB for the sensors 00073 float aRes, gRes; 00074 00075 // Calculates Acc resolution 00076 void MPU6050::getAres() 00077 { 00078 switch(Ascale) 00079 { 00080 case AFS_2G: 00081 aRes = 2.0/32768.0; 00082 break; 00083 case AFS_4G: 00084 aRes = 4.0/32768.0; 00085 break; 00086 case AFS_8G: 00087 aRes = 8.0/32768.0; 00088 break; 00089 case AFS_16G: 00090 aRes = 16.0/32768.0; 00091 break; 00092 } 00093 } 00094 00095 // Calculates Gyro resolution 00096 void MPU6050::getGres() 00097 { 00098 switch(Gscale) 00099 { 00100 case GFS_250DPS: 00101 gRes = 250.0/32768.0; 00102 break; 00103 case GFS_500DPS: 00104 gRes = 500.0/32768.0; 00105 break; 00106 case GFS_1000DPS: 00107 gRes = 1000.0/32768.0; 00108 break; 00109 case GFS_2000DPS: 00110 gRes = 2000.0/32768.0; 00111 break; 00112 } 00113 } 00114 00115 void MPU6050::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) 00116 { 00117 char data_write[2]; 00118 data_write[0]=subAddress; // I2C sends MSB first. Namely >>|subAddress|>>|data| 00119 data_write[1]=data; 00120 i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false); 00121 } 00122 00123 char MPU6050::readByte(uint8_t address, uint8_t subAddress) 00124 { 00125 char data_read[1]; // will store the register data 00126 char data_write[1]; 00127 data_write[0]=subAddress; 00128 i2c.write(address,data_write,1,1); // have not stopped yet 00129 i2c.read(address,data_read,1,0); // read the data and stop 00130 return data_read[0]; 00131 } 00132 00133 void MPU6050::readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest) 00134 { 00135 char data[14],data_write[1]; 00136 data_write[0]=subAddress; 00137 i2c.write(address,data_write,1,1); 00138 i2c.read(address,data,byteNum,0); 00139 for(int i=0;i<byteNum;i++) // equate the addresses 00140 dest[i]=data[i]; 00141 } 00142 00143 // Communication test: WHO_AM_I register reading 00144 void MPU6050::whoAmI() 00145 { 00146 uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Should return 0x68 00147 pc.printf("I AM 0x%x \r\n",whoAmI); 00148 00149 if(whoAmI==0x68) 00150 { 00151 pc.printf("MPU6050 is online... \r\n"); 00152 //led2=1; 00153 } 00154 else 00155 { 00156 pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n"); 00157 //toggler1.attach(&toggle_led1,0.1); // toggles led1 every 100 ms 00158 } 00159 } 00160 00161 // Initializes MPU6050 with the following config: 00162 // PLL with X axis gyroscope reference 00163 // Sample rate: 200Hz for gyro and acc 00164 // Interrupts are disabled 00165 void MPU6050::init() 00166 { 00167 i2c.frequency(400000); // fast i2c: 400 kHz 00168 00169 /* Wake up the device */ 00170 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // wake up the device by clearing the sleep bit (bit6) 00171 wait_ms(100); // wait 100 ms to stabilize 00172 00173 /* Get stable time source */ 00174 // PLL with X axis gyroscope reference is used to improve stability 00175 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); 00176 00177 /* Configure Gyroscope and Accelerometer */ 00178 // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz 00179 // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate) 00180 writeByte(MPU6050_ADDRESS, CONFIG, 0x03); 00181 00182 /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */ 00183 // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above) 00184 writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); 00185 00186 /* Accelerometer configuration */ 00187 uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); 00188 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] 00189 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0x18); // Clear AFS bits [4:3] 00190 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp | Ascale<<3); // Set full scale range 00191 00192 /* Gyroscope configuration */ 00193 temp = readByte(MPU6050_ADDRESS, GYRO_CONFIG); 00194 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] 00195 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18); // Clear FS bits [4:3] 00196 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3); // Set full scale range 00197 } 00198 00199 // Resets the device 00200 void MPU6050::reset() 00201 { 00202 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // set bit7 to reset the device 00203 wait_ms(100); // wait 100 ms to stabilize 00204 } 00205 00206 void MPU6050::readAccelData(int16_t* dest) 00207 { 00208 uint8_t rawData[6]; // x,y,z acc data 00209 readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // read six raw data registers sequentially and write them into data array 00210 00211 /* Turn the MSB LSB into signed 16-bit value */ 00212 dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // ACCEL_XOUT 00213 dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // ACCEL_YOUT 00214 dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // ACCEL_ZOUT 00215 } 00216 00217 void MPU6050::readGyroData(int16_t* dest) 00218 { 00219 uint8_t rawData[6]; // x,y,z gyro data 00220 readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // read the six raw data registers sequentially and write them into data array 00221 00222 /* Turn the MSB LSB into signed 16-bit value */ 00223 dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // GYRO_XOUT 00224 dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // GYRO_YOUT 00225 dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // GYRO_ZOUT 00226 } 00227 00228 int16_t MPU6050::readTempData() 00229 { 00230 uint8_t rawData[2]; // temperature data 00231 readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // read the two raw data registers sequentially and write them into data array 00232 return (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // turn the MSB LSB into signed 16-bit value 00233 } 00234 00235 /* Function which accumulates gyro and accelerometer data after device initialization. 00236 It calculates the average of the at-rest readings and 00237 then loads the resulting offsets into accelerometer and gyro bias registers. */ 00238 /* 00239 IMPORTANT NOTE: In this function; 00240 Resulting accel offsets are NOT pushed to the accel bias registers. accelBias[i] offsets are used in the main program. 00241 Resulting gyro offsets are pushed to the gyro bias registers. gyroBias[i] offsets are NOT used in the main program. 00242 Resulting data seems satisfactory. 00243 */ 00244 // dest1: accelBias dest2: gyroBias 00245 void MPU6050::calibrate(float* dest1, float* dest2) 00246 { 00247 uint8_t data[12]; // data array to hold acc and gyro x,y,z data 00248 uint16_t fifo_count, packet_count, count; 00249 int32_t accel_bias[3] = {0,0,0}; 00250 int32_t gyro_bias[3] = {0,0,0}; 00251 float aRes = 2.0/32768.0; 00252 float gRes = 250.0/32768.0; 00253 uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g 00254 //uint16_t gyrosensitivity = 131; // = 1/gRes = 131 LSB/dps 00255 00256 reset(); // Reset device 00257 00258 /* Get stable time source */ 00259 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // PLL with X axis gyroscope reference is used to improve stability 00260 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); // Disable accel only low power mode 00261 wait(0.2); 00262 00263 /* Configure device for bias calculation */ 00264 writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts 00265 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO 00266 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source 00267 writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master 00268 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes 00269 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x04); // Reset FIFO 00270 wait(0.015); 00271 00272 /* Configure accel and gyro for bias calculation */ 00273 writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz 00274 writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz 00275 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity 00276 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity 00277 00278 /* Configure FIFO to capture accelerometer and gyro data for bias calculation */ 00279 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO 00280 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable accelerometer and gyro for FIFO (max size 1024 bytes in MPU-6050) 00281 wait(0.08); // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds. 00282 // accX: 2 byte, accY: 2 byte, accZ: 2 byte. gyroX: 2 byte, gyroY: 2 byte, gyroZ: 2 byte. 12*80=960 byte < 1024 byte 00283 00284 /* At end of sample accumulation, turn off FIFO sensor read */ 00285 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO 00286 readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // Read FIFO sample count 00287 fifo_count = ((uint16_t)data[0] << 8) | data[1]; 00288 packet_count = fifo_count/12; // The number of sets of full acc and gyro data for averaging. packet_count = 80 in this case 00289 00290 for(count=0; count<packet_count; count++) 00291 { 00292 int16_t accel_temp[3]={0,0,0}; 00293 int16_t gyro_temp[3]={0,0,0}; 00294 readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging 00295 00296 /* Form signed 16-bit integer for each sample in FIFO */ 00297 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; 00298 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; 00299 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; 00300 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; 00301 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; 00302 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; 00303 00304 /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */ 00305 accel_bias[0] += (int32_t) accel_temp[0]; 00306 accel_bias[1] += (int32_t) accel_temp[1]; 00307 accel_bias[2] += (int32_t) accel_temp[2]; 00308 gyro_bias[0] += (int32_t) gyro_temp[0]; 00309 gyro_bias[1] += (int32_t) gyro_temp[1]; 00310 gyro_bias[2] += (int32_t) gyro_temp[2]; 00311 } 00312 00313 /* Normalize sums to get average count biases */ 00314 accel_bias[0] /= (int32_t) packet_count; 00315 accel_bias[1] /= (int32_t) packet_count; 00316 accel_bias[2] /= (int32_t) packet_count; 00317 gyro_bias[0] /= (int32_t) packet_count; 00318 gyro_bias[1] /= (int32_t) packet_count; 00319 gyro_bias[2] /= (int32_t) packet_count; 00320 00321 /* Remove gravity from the z-axis accelerometer bias calculation */ 00322 if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;} 00323 else {accel_bias[2] += (int32_t) accelsensitivity;} 00324 00325 /* Output scaled accelerometer biases for manual subtraction in the main program */ 00326 dest1[0] = accel_bias[0]*aRes; 00327 dest1[1] = accel_bias[1]*aRes; 00328 dest1[2] = accel_bias[2]*aRes; 00329 00330 /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */ 00331 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 00332 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases 00333 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; 00334 data[3] = (-gyro_bias[1]/4) & 0xFF; 00335 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; 00336 data[5] = (-gyro_bias[2]/4) & 0xFF; 00337 00338 /* Push gyro biases to hardware registers */ 00339 writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); 00340 writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]); 00341 writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]); 00342 writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]); 00343 writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]); 00344 writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]); 00345 00346 /* Construct gyro bias in deg/s for later manual subtraction */ 00347 dest2[0] = gyro_bias[0]*gRes; 00348 dest2[1] = gyro_bias[1]*gRes; 00349 dest2[2] = gyro_bias[2]*gRes; 00350 } 00351 00352 void MPU6050::complementaryFilter(float* pitch, float* roll) 00353 { 00354 /* Get actual acc value */ 00355 readAccelData(accelData); 00356 getAres(); 00357 ax = accelData[0]*aRes - accelBias[0]; 00358 ay = accelData[1]*aRes - accelBias[1]; 00359 az = accelData[2]*aRes - accelBias[2]; 00360 00361 /* Get actual gyro value */ 00362 readGyroData(gyroData); 00363 getGres(); 00364 gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i] 00365 gy = gyroData[1]*gRes; // - gyroBias[1]; 00366 gz = gyroData[2]*gRes; // - gyroBias[2]; 00367 00368 float pitchAcc, rollAcc,roll1, pitch1; 00369 00370 /* Integrate the gyro data(deg/s) over time to get angle */ 00371 *pitch += gx * dt; // Angle around the X-axis 00372 *roll -= gy * dt; // Angle around the Y-axis 00373 float pt1 = (accelData[0]*accelData[0]) + (accelData[2]*accelData[2]); 00374 float pt2 = (accelData[1]*accelData[1]) + (accelData[2]*accelData[2]); 00375 roll1 = atan2f(accelData[0], sqrt(pt1)); 00376 pitch1 = atan2f(accelData[1], sqrt(pt2)); 00377 00378 /* Turning around the X-axis results in a vector on the Y-axis 00379 whereas turning around the Y-axis results in a vector on the X-axis. */ 00380 pitchAcc = atan2f(accelData[1], accelData[2])*180/PI; 00381 rollAcc = atan2f(accelData[0], accelData[2])*180/PI; 00382 00383 /* Apply Complementary Filter */ 00384 *pitch = *pitch * 0.98 + pitchAcc * 0.02; 00385 *roll = *roll * 0.98 + rollAcc * 0.02; 00386 }
Generated on Tue Jul 12 2022 23:14:00 by
1.7.2