UAVRO / MPU6050

MPU6050 library using i2c interface on LPC1768 - Complementary filter is added. Now program can calculate pitch and roll angles.

Dependents:   Q2_Stabi

Fork of MPU6050 by Baser Kandehir

MPU6050.cpp@7:4e799f8ec792, 2016-05-30 (annotated)

Committer:
Decimus
Date:
Mon May 30 08:11:07 2016 +0000
Revision:
7:4e799f8ec792
Parent:
6:5b90f2b5e6d9 
[+]

Who changed what in which revision?

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