Songw Jiang / MPU6050

Not a real MPU6050 but imc-20689

Fork of MPU6050 by Erik -

MPU6050.cpp@3:d2b927200037, 2018-10-23 (annotated)

Committer:
Ucial
Date:
Tue Oct 23 10:48:22 2018 +0000
Revision:
3:d2b927200037
Parent:
1:a3366f09e95c 
adf

Who changed what in which revision?

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