Not a real MPU6050 but imc-20689
Fork of MPU6050 by
Diff: MPU6050.cpp
- Revision:
- 3:d2b927200037
- Parent:
- 1:a3366f09e95c
diff -r 5c63e20c50f3 -r d2b927200037 MPU6050.cpp --- a/MPU6050.cpp Mon Sep 10 21:26:25 2012 +0000 +++ b/MPU6050.cpp Tue Oct 23 10:48:22 2018 +0000 @@ -1,237 +1,342 @@ -/** - * Includes - */ #include "MPU6050.h" -MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) { - this->setSleepMode(false); - - //Initializations: - currentGyroRange = 0; - currentAcceleroRange=0; -} +/* For LPC1768 board */ +//I2C i2c(p9,p10); // setup i2c (SDA,SCL) + +/* For NUCLEO-F411RE board */ +//static I2C i2c(D14,D15); // setup i2c (SDA,SCL) + +/* For Fire board */ +static I2C i2c(PB_11,PB_10); // setup i2c (SDA,SCL) + + +/* Set initial input parameters */ -//-------------------------------------------------- -//-------------------General------------------------ -//-------------------------------------------------- +// Acc Full Scale Range +-2G 4G 8G 16G +enum Ascale +{ + AFS_2G=0, + AFS_4G, + AFS_8G, + AFS_16G +}; -void MPU6050::write(char address, char data) { - char temp[2]; - temp[0]=address; - temp[1]=data; - - connection.write(MPU6050_ADDRESS * 2,temp,2); -} +// Gyro Full Scale Range +-250 500 1000 2000 Degrees per second +enum Gscale +{ + GFS_250DPS=0, + GFS_500DPS, + GFS_1000DPS, + GFS_2000DPS +}; -char MPU6050::read(char address) { - char retval; - connection.write(MPU6050_ADDRESS * 2, &address, 1, true); - connection.read(MPU6050_ADDRESS * 2, &retval, 1); - return retval; -} +// Sensor datas +float ax,ay,az; +float gx,gy,gz; +int16_t accelData[3],gyroData[3],tempData; +float accelBias[3] = {0, 0, 0}; // Bias corrections for acc +float gyroBias[3] = {0, 0, 0}; // Bias corrections for gyro -void MPU6050::read(char address, char *data, int length) { - connection.write(MPU6050_ADDRESS * 2, &address, 1, true); - connection.read(MPU6050_ADDRESS * 2, data, length); -} +// Specify sensor full scale range +int Ascale = AFS_2G; +int Gscale = GFS_250DPS; + +// Scale resolutions per LSB for the sensors +float aRes, gRes; -void MPU6050::setSleepMode(bool state) { - char temp; - temp = this->read(MPU6050_PWR_MGMT_1_REG); - if (state == true) - temp |= 1<<MPU6050_SLP_BIT; - if (state == false) - temp &= ~(1<<MPU6050_SLP_BIT); - this->write(MPU6050_PWR_MGMT_1_REG, temp); -} - -bool MPU6050::testConnection( void ) { - char temp; - temp = this->read(MPU6050_WHO_AM_I_REG); - return (temp == (MPU6050_ADDRESS & 0xFE)); +// Calculates Acc resolution +void MPU6050::getAres() +{ + switch(Ascale) + { + case AFS_2G: + aRes = 2.0/32768.0; + break; + case AFS_4G: + aRes = 4.0/32768.0; + break; + case AFS_8G: + aRes = 8.0/32768.0; + break; + case AFS_16G: + aRes = 16.0/32768.0; + break; + } } -void MPU6050::setBW(char BW) { - char temp; - BW=BW & 0x07; - temp = this->read(MPU6050_CONFIG_REG); - temp &= 0xF8; - temp = temp + BW; - this->write(MPU6050_CONFIG_REG, temp); +// Calculates Gyro resolution +void MPU6050::getGres() +{ + switch(Gscale) + { + case GFS_250DPS: + gRes = 250.0/32768.0; + break; + case GFS_500DPS: + gRes = 500.0/32768.0; + break; + case GFS_1000DPS: + gRes = 1000.0/32768.0; + break; + case GFS_2000DPS: + gRes = 2000.0/32768.0; + break; + } +} + +void MPU6050::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) +{ + char data_write[2]; + data_write[0]=subAddress; // I2C sends MSB first. Namely >>|subAddress|>>|data| + data_write[1]=data; + i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false); } -void MPU6050::setI2CBypass(bool state) { - char temp; - temp = this->read(MPU6050_INT_PIN_CFG); - if (state == true) - temp |= 1<<MPU6050_BYPASS_BIT; - if (state == false) - temp &= ~(1<<MPU6050_BYPASS_BIT); - this->write(MPU6050_INT_PIN_CFG, temp); +char MPU6050::readByte(uint8_t address, uint8_t subAddress) +{ + char data_read[1]; // will store the register data + char data_write[1]; + data_write[0]=subAddress; + i2c.write(address,data_write,1,1); // have not stopped yet + i2c.read(address,data_read,1,0); // read the data and stop + return data_read[0]; +} + +void MPU6050::readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest) +{ + char data[14],data_write[1]; + data_write[0]=subAddress; + i2c.write(address,data_write,1,1); + i2c.read(address,data,byteNum,0); + for(int i=0;i<byteNum;i++) // equate the addresses + dest[i]=data[i]; } -//-------------------------------------------------- -//----------------Accelerometer--------------------- -//-------------------------------------------------- - -void MPU6050::setAcceleroRange( char range ) { - char temp; - range = range & 0x03; - currentAcceleroRange = range; - - temp = this->read(MPU6050_ACCELERO_CONFIG_REG); - temp &= ~(3<<3); - temp = temp + (range<<3); - this->write(MPU6050_ACCELERO_CONFIG_REG, temp); +// Communication test: WHO_AM_I register reading +void MPU6050::whoAmI() +{ + uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Should return 0x68 } -int MPU6050::getAcceleroRawX( void ) { - short retval; - char data[2]; - this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2); - retval = (data[0]<<8) + data[1]; - return (int)retval; -} +// Initializes MPU6050 with the following config: +// PLL with X axis gyroscope reference +// Sample rate: 200Hz for gyro and acc +// Interrupts are disabled +void MPU6050::init() +{ + i2c.frequency(400000); // fast i2c: 400 kHz + + /* Wake up the device */ + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // wake up the device by clearing the sleep bit (bit6) + wait_ms(100); // wait 100 ms to stabilize + + /* Get stable time source */ + // PLL with X axis gyroscope reference is used to improve stability + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); -int MPU6050::getAcceleroRawY( void ) { - short retval; - char data[2]; - this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2); - retval = (data[0]<<8) + data[1]; - return (int)retval; + /* Configure Gyroscope and Accelerometer */ + // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz + // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate) + writeByte(MPU6050_ADDRESS, CONFIG, 0x03); + + /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */ + // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above) + writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); + + /* Accelerometer configuration */ + uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0x18); // Clear AFS bits [4:3] + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp | Ascale<<3); // Set full scale range + + /* Gyroscope configuration */ + temp = readByte(MPU6050_ADDRESS, GYRO_CONFIG); + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5] + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18); // Clear FS bits [4:3] + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3); // Set full scale range } -int MPU6050::getAcceleroRawZ( void ) { - short retval; - char data[2]; - this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2); - retval = (data[0]<<8) + data[1]; - return (int)retval; -} - -void MPU6050::getAcceleroRaw( int *data ) { - char temp[6]; - this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6); - data[0] = (int)(short)((temp[0]<<8) + temp[1]); - data[1] = (int)(short)((temp[2]<<8) + temp[3]); - data[2] = (int)(short)((temp[4]<<8) + temp[5]); +// Resets the device +void MPU6050::reset() +{ + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // set bit7 to reset the device + wait_ms(100); // wait 100 ms to stabilize } -void MPU6050::getAccelero( float *data ) { - int temp[3]; - this->getAcceleroRaw(temp); - if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) { - data[0]=(float)temp[0] / 16384.0 * 9.81; - data[1]=(float)temp[1] / 16384.0 * 9.81; - data[2]=(float)temp[2] / 16384.0 * 9.81; - } - if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){ - data[0]=(float)temp[0] / 8192.0 * 9.81; - data[1]=(float)temp[1] / 8192.0 * 9.81; - data[2]=(float)temp[2] / 8192.0 * 9.81; - } - if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){ - data[0]=(float)temp[0] / 4096.0 * 9.81; - data[1]=(float)temp[1] / 4096.0 * 9.81; - data[2]=(float)temp[2] / 4096.0 * 9.81; - } - if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){ - data[0]=(float)temp[0] / 2048.0 * 9.81; - data[1]=(float)temp[1] / 2048.0 * 9.81; - data[2]=(float)temp[2] / 2048.0 * 9.81; - } +void MPU6050::readAccelData(int16_t* dest) +{ + uint8_t rawData[6]; // x,y,z acc data + readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // read six raw data registers sequentially and write them into data array - #ifdef DOUBLE_ACCELERO - data[0]*=2; - data[1]*=2; - data[2]*=2; - #endif + /* Turn the MSB LSB into signed 16-bit value */ + dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // ACCEL_XOUT + dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // ACCEL_YOUT + dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // ACCEL_ZOUT } -//-------------------------------------------------- -//------------------Gyroscope----------------------- -//-------------------------------------------------- -void MPU6050::setGyroRange( char range ) { - char temp; - currentGyroRange = range; - range = range & 0x03; - temp = this->read(MPU6050_GYRO_CONFIG_REG); - temp &= ~(3<<3); - temp = temp + range<<3; - this->write(MPU6050_GYRO_CONFIG_REG, temp); -} - -int MPU6050::getGyroRawX( void ) { - short retval; - char data[2]; - this->read(MPU6050_GYRO_XOUT_H_REG, data, 2); - retval = (data[0]<<8) + data[1]; - return (int)retval; +void MPU6050::readGyroData(int16_t* dest) +{ + uint8_t rawData[6]; // x,y,z gyro data + readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // read the six raw data registers sequentially and write them into data array + + /* Turn the MSB LSB into signed 16-bit value */ + dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // GYRO_XOUT + dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // GYRO_YOUT + dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // GYRO_ZOUT } -int MPU6050::getGyroRawY( void ) { - short retval; - char data[2]; - this->read(MPU6050_GYRO_YOUT_H_REG, data, 2); - retval = (data[0]<<8) + data[1]; - return (int)retval; -} - -int MPU6050::getGyroRawZ( void ) { - short retval; - char data[2]; - this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2); - retval = (data[0]<<8) + data[1]; - return (int)retval; -} - -void MPU6050::getGyroRaw( int *data ) { - char temp[6]; - this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6); - data[0] = (int)(short)((temp[0]<<8) + temp[1]); - data[1] = (int)(short)((temp[2]<<8) + temp[3]); - data[2] = (int)(short)((temp[4]<<8) + temp[5]); +int16_t MPU6050::readTempData() +{ + uint8_t rawData[2]; // temperature data + readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // read the two raw data registers sequentially and write them into data array + return (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // turn the MSB LSB into signed 16-bit value } -void MPU6050::getGyro( float *data ) { - int temp[3]; - this->getGyroRaw(temp); - if (currentGyroRange == MPU6050_GYRO_RANGE_250) { - data[0]=(float)temp[0] / 7505.7; - data[1]=(float)temp[1] / 7505.7; - data[2]=(float)temp[2] / 7505.7; - } - if (currentGyroRange == MPU6050_GYRO_RANGE_500){ - data[0]=(float)temp[0] / 3752.9; - data[1]=(float)temp[1] / 3752.9; - data[2]=(float)temp[2] / 3752.9; - } - if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ - data[0]=(float)temp[0] / 1879.3;; - data[1]=(float)temp[1] / 1879.3; - data[2]=(float)temp[2] / 1879.3; - } - if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ - data[0]=(float)temp[0] / 939.7; - data[1]=(float)temp[1] / 939.7; - data[2]=(float)temp[2] / 939.7; - } -} -//-------------------------------------------------- -//-------------------Temperature-------------------- -//-------------------------------------------------- -int MPU6050::getTempRaw( void ) { - short retval; - char data[2]; - this->read(MPU6050_TEMP_H_REG, data, 2); - retval = (data[0]<<8) + data[1]; - return (int)retval; +/* Function which accumulates gyro and accelerometer data after device initialization. + It calculates the average of the at-rest readings and + then loads the resulting offsets into accelerometer and gyro bias registers. */ +/* + IMPORTANT NOTE: In this function; + Resulting accel offsets are NOT pushed to the accel bias registers. accelBias[i] offsets are used in the main program. + Resulting gyro offsets are pushed to the gyro bias registers. gyroBias[i] offsets are NOT used in the main program. + Resulting data seems satisfactory. +*/ +// dest1: accelBias dest2: gyroBias +void MPU6050::calibrate(float* dest1, float* dest2) +{ + uint8_t data[12]; // data array to hold acc and gyro x,y,z data + uint16_t fifo_count, packet_count, count; + int32_t accel_bias[3] = {0,0,0}; + int32_t gyro_bias[3] = {0,0,0}; + float aRes = 2.0/32768.0; + float gRes = 250.0/32768.0; + uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g + //uint16_t gyrosensitivity = 131; // = 1/gRes = 131 LSB/dps + + reset(); // Reset device + + /* Get stable time source */ + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // PLL with X axis gyroscope reference is used to improve stability + writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); // Disable accel only low power mode + wait(0.2); + + /* Configure device for bias calculation */ + writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts + writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source + writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master + writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes + writeByte(MPU6050_ADDRESS, USER_CTRL, 0x04); // Reset FIFO + wait(0.015); + + /* Configure accel and gyro for bias calculation */ + writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz + writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + + /* Configure FIFO to capture accelerometer and gyro data for bias calculation */ + writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO + writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable accelerometer and gyro for FIFO (max size 1024 bytes in MPU-6050) + wait(0.08); // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds. + // accX: 2 byte, accY: 2 byte, accZ: 2 byte. gyroX: 2 byte, gyroY: 2 byte, gyroZ: 2 byte. 12*80=960 byte < 1024 byte + + /* At end of sample accumulation, turn off FIFO sensor read */ + writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO + readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // Read FIFO sample count + fifo_count = ((uint16_t)data[0] << 8) | data[1]; + packet_count = fifo_count/12; // The number of sets of full acc and gyro data for averaging. packet_count = 80 in this case + + for(count=0; count<packet_count; count++) + { + int16_t accel_temp[3]={0,0,0}; + int16_t gyro_temp[3]={0,0,0}; + readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging + + /* Form signed 16-bit integer for each sample in FIFO */ + accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; + accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; + accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; + gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; + gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; + gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; + + /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */ + accel_bias[0] += (int32_t) accel_temp[0]; + accel_bias[1] += (int32_t) accel_temp[1]; + accel_bias[2] += (int32_t) accel_temp[2]; + gyro_bias[0] += (int32_t) gyro_temp[0]; + gyro_bias[1] += (int32_t) gyro_temp[1]; + gyro_bias[2] += (int32_t) gyro_temp[2]; + } + + /* Normalize sums to get average count biases */ + accel_bias[0] /= (int32_t) packet_count; + accel_bias[1] /= (int32_t) packet_count; + accel_bias[2] /= (int32_t) packet_count; + gyro_bias[0] /= (int32_t) packet_count; + gyro_bias[1] /= (int32_t) packet_count; + gyro_bias[2] /= (int32_t) packet_count; + + /* Remove gravity from the z-axis accelerometer bias calculation */ + if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;} + else {accel_bias[2] += (int32_t) accelsensitivity;} + + /* Output scaled accelerometer biases for manual subtraction in the main program */ + dest1[0] = accel_bias[0]*aRes; + dest1[1] = accel_bias[1]*aRes; + dest1[2] = accel_bias[2]*aRes; + + /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */ + 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 + data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases + data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; + data[3] = (-gyro_bias[1]/4) & 0xFF; + data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; + data[5] = (-gyro_bias[2]/4) & 0xFF; + + /* Push gyro biases to hardware registers */ + writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); + writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]); + writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]); + writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]); + writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]); + writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]); + + /* Construct gyro bias in deg/s for later manual subtraction */ + dest2[0] = gyro_bias[0]*gRes; + dest2[1] = gyro_bias[1]*gRes; + dest2[2] = gyro_bias[2]*gRes; } -float MPU6050::getTemp( void ) { - float retval; - retval=(float)this->getTempRaw(); - retval=(retval+521.0)/340.0+35.0; - return retval; +void MPU6050::complementaryFilter(float* pitch, float* roll) +{ + /* Get actual acc value */ + readAccelData(accelData); + getAres(); + ax = accelData[0]*aRes - accelBias[0]; + ay = accelData[1]*aRes - accelBias[1]; + az = accelData[2]*aRes - accelBias[2]; + + /* Get actual gyro value */ + readGyroData(gyroData); + getGres(); + gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i] + gy = gyroData[1]*gRes; // - gyroBias[1]; + gz = gyroData[2]*gRes; // - gyroBias[2]; + + float pitchAcc, rollAcc; + + /* Integrate the gyro data(deg/s) over time to get angle */ + *pitch += gx * dt; // Angle around the X-axis + *roll -= gy * dt; // Angle around the Y-axis + + /* Turning around the X-axis results in a vector on the Y-axis + whereas turning around the Y-axis results in a vector on the X-axis. */ + pitchAcc = atan2f(accelData[1], accelData[2])*180/PI; + rollAcc = atan2f(accelData[0], accelData[2])*180/PI; + + /* Apply Complementary Filter */ + *pitch = *pitch * 0.98 + pitchAcc * 0.02; + *roll = *roll * 0.98 + rollAcc * 0.02; } -