Sérgio Natan / Mbed OS luva_tatil_repo

teste

Dependencies:   BurstSPI Fonts

MPU6050.cpp@0:cf17b1f16335, 2017-10-24 (annotated)

Committer:
sergionatan
Date:
Tue Oct 24 20:12:54 2017 +0000
Revision:
0:cf17b1f16335
Initial commit

Who changed what in which revision?

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