Songw Jiang
Not a real MPU6050 but imc-20689
Revision 3:d2b927200037, committed 2018-10-23
- Comitter:
- Ucial
- Date:
- Tue Oct 23 10:48:22 2018 +0000
- Parent:
- 2:5c63e20c50f3
- Commit message:
- adf
Changed in this revision
| MPU6050.cpp | Show annotated file Show diff for this revision Revisions of this file |
| MPU6050.h | Show annotated file Show diff for this revision Revisions of this file |
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;
}
-
diff -r 5c63e20c50f3 -r d2b927200037 MPU6050.h
--- a/MPU6050.h Mon Sep 10 21:26:25 2012 +0000
+++ b/MPU6050.h Tue Oct 23 10:48:22 2018 +0000
@@ -1,274 +1,166 @@
-/*Use #define MPU6050_ES before you include this file if you have an engineering sample (older EVBs will have them), to find out if you have one, check your accelerometer output.
-If it is half of what you expected, and you still are on the correct planet, you got an engineering sample
-*/
-
-
#ifndef MPU6050_H
#define MPU6050_H
-/**
- * Includes
- */
#include "mbed.h"
+#include "math.h"
+#define PI 3.14159265359 // This value will be used when calculating angles
+#define dt 0.005 // 200 Hz sampling period
-/**
- * Defines
- */
-#ifndef MPU6050_ADDRESS
- #define MPU6050_ADDRESS 0x69 // address pin low (GND), default for InvenSense evaluation board
+#define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD
+#define YGOFFS_TC 0x01
+#define ZGOFFS_TC 0x02
+#define X_FINE_GAIN 0x03 // [7:0] fine gain
+#define Y_FINE_GAIN 0x04
+#define Z_FINE_GAIN 0x05
+#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
+#define XA_OFFSET_L_TC 0x07
+#define YA_OFFSET_H 0x08
+#define YA_OFFSET_L_TC 0x09
+#define ZA_OFFSET_H 0x0A
+#define ZA_OFFSET_L_TC 0x0B
+#define SELF_TEST_X 0x0D
+#define SELF_TEST_Y 0x0E
+#define SELF_TEST_Z 0x0F
+#define SELF_TEST_A 0x10
+#define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050?
+#define XG_OFFS_USRL 0x14
+#define YG_OFFS_USRH 0x15
+#define YG_OFFS_USRL 0x16
+#define ZG_OFFS_USRH 0x17
+#define ZG_OFFS_USRL 0x18
+#define SMPLRT_DIV 0x19
+#define CONFIG 0x1A
+#define GYRO_CONFIG 0x1B
+#define ACCEL_CONFIG 0x1C
+#define FF_THR 0x1D // Free-fall
+#define FF_DUR 0x1E // Free-fall
+#define MOT_THR 0x1F // Motion detection threshold bits [7:0]
+#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
+#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
+#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
+#define FIFO_EN 0x23
+#define I2C_MST_CTRL 0x24
+#define I2C_SLV0_ADDR 0x25
+#define I2C_SLV0_REG 0x26
+#define I2C_SLV0_CTRL 0x27
+#define I2C_SLV1_ADDR 0x28
+#define I2C_SLV1_REG 0x29
+#define I2C_SLV1_CTRL 0x2A
+#define I2C_SLV2_ADDR 0x2B
+#define I2C_SLV2_REG 0x2C
+#define I2C_SLV2_CTRL 0x2D
+#define I2C_SLV3_ADDR 0x2E
+#define I2C_SLV3_REG 0x2F
+#define I2C_SLV3_CTRL 0x30
+#define I2C_SLV4_ADDR 0x31
+#define I2C_SLV4_REG 0x32
+#define I2C_SLV4_DO 0x33
+#define I2C_SLV4_CTRL 0x34
+#define I2C_SLV4_DI 0x35
+#define I2C_MST_STATUS 0x36
+#define INT_PIN_CFG 0x37
+#define INT_ENABLE 0x38
+#define DMP_INT_STATUS 0x39 // Check DMP interrupt
+#define INT_STATUS 0x3A
+#define ACCEL_XOUT_H 0x3B
+#define ACCEL_XOUT_L 0x3C
+#define ACCEL_YOUT_H 0x3D
+#define ACCEL_YOUT_L 0x3E
+#define ACCEL_ZOUT_H 0x3F
+#define ACCEL_ZOUT_L 0x40
+#define TEMP_OUT_H 0x41
+#define TEMP_OUT_L 0x42
+#define GYRO_XOUT_H 0x43
+#define GYRO_XOUT_L 0x44
+#define GYRO_YOUT_H 0x45
+#define GYRO_YOUT_L 0x46
+#define GYRO_ZOUT_H 0x47
+#define GYRO_ZOUT_L 0x48
+#define EXT_SENS_DATA_00 0x49
+#define EXT_SENS_DATA_01 0x4A
+#define EXT_SENS_DATA_02 0x4B
+#define EXT_SENS_DATA_03 0x4C
+#define EXT_SENS_DATA_04 0x4D
+#define EXT_SENS_DATA_05 0x4E
+#define EXT_SENS_DATA_06 0x4F
+#define EXT_SENS_DATA_07 0x50
+#define EXT_SENS_DATA_08 0x51
+#define EXT_SENS_DATA_09 0x52
+#define EXT_SENS_DATA_10 0x53
+#define EXT_SENS_DATA_11 0x54
+#define EXT_SENS_DATA_12 0x55
+#define EXT_SENS_DATA_13 0x56
+#define EXT_SENS_DATA_14 0x57
+#define EXT_SENS_DATA_15 0x58
+#define EXT_SENS_DATA_16 0x59
+#define EXT_SENS_DATA_17 0x5A
+#define EXT_SENS_DATA_18 0x5B
+#define EXT_SENS_DATA_19 0x5C
+#define EXT_SENS_DATA_20 0x5D
+#define EXT_SENS_DATA_21 0x5E
+#define EXT_SENS_DATA_22 0x5F
+#define EXT_SENS_DATA_23 0x60
+#define MOT_DETECT_STATUS 0x61
+#define I2C_SLV0_DO 0x63
+#define I2C_SLV1_DO 0x64
+#define I2C_SLV2_DO 0x65
+#define I2C_SLV3_DO 0x66
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET 0x68
+#define MOT_DETECT_CTRL 0x69
+#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
+#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
+#define PWR_MGMT_2 0x6C
+#define DMP_BANK 0x6D // Activates a specific bank in the DMP
+#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
+#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
+#define DMP_REG_1 0x70
+#define DMP_REG_2 0x71
+#define FIFO_COUNTH 0x72
+#define FIFO_COUNTL 0x73
+#define FIFO_R_W 0x74
+#define WHO_AM_I_MPU6050 0x75 // Should return 0x68
+
+// Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor
+// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
+#define ADO 0
+#if ADO
+#define MPU6050_ADDRESS 0x69<<1 // Device address when ADO = 1
+#else
+#define MPU6050_ADDRESS 0x68<<1 // Device address when ADO = 0
#endif
-#ifdef MPU6050_ES
- #define DOUBLE_ACCELERO
-#endif
-
-/**
- * Registers
- */
- #define MPU6050_CONFIG_REG 0x1A
- #define MPU6050_GYRO_CONFIG_REG 0x1B
- #define MPU6050_ACCELERO_CONFIG_REG 0x1C
-
- #define MPU6050_INT_PIN_CFG 0x37
-
- #define MPU6050_ACCEL_XOUT_H_REG 0x3B
- #define MPU6050_ACCEL_YOUT_H_REG 0x3D
- #define MPU6050_ACCEL_ZOUT_H_REG 0x3F
-
- #define MPU6050_TEMP_H_REG 0x41
-
- #define MPU6050_GYRO_XOUT_H_REG 0x43
- #define MPU6050_GYRO_YOUT_H_REG 0x45
- #define MPU6050_GYRO_ZOUT_H_REG 0x47
-
-
-
- #define MPU6050_PWR_MGMT_1_REG 0x6B
- #define MPU6050_WHO_AM_I_REG 0x75
-
-
-
- /**
- * Definitions
- */
-#define MPU6050_SLP_BIT 6
-#define MPU6050_BYPASS_BIT 1
+extern float aRes, gRes;
-#define MPU6050_BW_256 0
-#define MPU6050_BW_188 1
-#define MPU6050_BW_98 2
-#define MPU6050_BW_42 3
-#define MPU6050_BW_20 4
-#define MPU6050_BW_10 5
-#define MPU6050_BW_5 6
-
-#define MPU6050_ACCELERO_RANGE_2G 0
-#define MPU6050_ACCELERO_RANGE_4G 1
-#define MPU6050_ACCELERO_RANGE_8G 2
-#define MPU6050_ACCELERO_RANGE_16G 3
-
-#define MPU6050_GYRO_RANGE_250 0
-#define MPU6050_GYRO_RANGE_500 1
-#define MPU6050_GYRO_RANGE_1000 2
-#define MPU6050_GYRO_RANGE_2000 3
-
+/* whoAmI func uses this func, variables etc */
-/** MPU6050 IMU library.
- *
- * Example:
- * @code
- * Later, maybe
- * @endcode
- */
-class MPU6050 {
- public:
- /**
- * Constructor.
- *
- * Sleep mode of MPU6050 is immediatly disabled
- *
- * @param sda - mbed pin to use for the SDA I2C line.
- * @param scl - mbed pin to use for the SCL I2C line.
- */
- MPU6050(PinName sda, PinName scl);
-
+extern Serial pc;
- /**
- * Tests the I2C connection by reading the WHO_AM_I register.
- *
- * @return True for a working connection, false for an error
- */
- bool testConnection( void );
-
- /**
- * Sets the bandwidth of the digital low-pass filter
- *
- * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5
- * Last number is the gyro's BW in Hz (accelero BW is virtually identical)
- *
- * @param BW - The three bits that set the bandwidth (use the predefined macros)
- */
- void setBW( char BW );
-
- /**
- * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU6050 (useful for eval board, otherwise just connect them to primary I2C bus)
- *
- * @param state - Enables/disables the I2C bypass mode
- */
- void setI2CBypass ( bool state );
-
- /**
- * Sets the Accelero full-scale range
- *
- * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G
- *
- * @param range - The two bits that set the full-scale range (use the predefined macros)
- */
- void setAcceleroRange(char range);
-
- /**
- * Reads the accelero x-axis.
- *
- * @return 16-bit signed integer x-axis accelero data
- */
- int getAcceleroRawX( void );
-
- /**
- * Reads the accelero y-axis.
- *
- * @return 16-bit signed integer y-axis accelero data
- */
- int getAcceleroRawY( void );
-
- /**
- * Reads the accelero z-axis.
- *
- * @return 16-bit signed integer z-axis accelero data
- */
- int getAcceleroRawZ( void );
-
- /**
- * Reads all accelero data.
- *
- * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
- */
- void getAcceleroRaw( int *data );
-
- /**
- * Reads all accelero data, gives the acceleration in m/s2
- *
- * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
- *
- * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
- */
- void getAccelero( float *data );
-
- /**
- * Sets the Gyro full-scale range
- *
- * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000
- *
- * @param range - The two bits that set the full-scale range (use the predefined macros)
- */
- void setGyroRange(char range);
-
- /**
- * Reads the gyro x-axis.
- *
- * @return 16-bit signed integer x-axis gyro data
- */
- int getGyroRawX( void );
-
- /**
- * Reads the gyro y-axis.
- *
- * @return 16-bit signed integer y-axis gyro data
- */
- int getGyroRawY( void );
-
- /**
- * Reads the gyro z-axis.
- *
- * @return 16-bit signed integer z-axis gyro data
- */
- int getGyroRawZ( void );
-
- /**
- * Reads all gyro data.
- *
- * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
- */
- void getGyroRaw( int *data );
-
- /**
- * Reads all gyro data, gives the gyro in rad/s
- *
- * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
- *
- * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
- */
- void getGyro( float *data);
-
- /**
- * Reads temperature data.
- *
- * @return 16 bit signed integer with the raw temperature register value
- */
- int getTempRaw( void );
-
- /**
- * Returns current temperature
- *
- * @returns float with the current temperature
- */
- float getTemp( void );
-
- /**
- * Sets the sleep mode of the MPU6050
- *
- * @param state - true for sleeping, false for wake up
- */
- void setSleepMode( bool state );
-
-
- /**
- * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU.
- *
- * @param adress - register address to write to
- * @param data - data to write
- */
- void write( char address, char data);
-
- /**
- * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU.
- *
- * @param adress - register address to write to
- * @return - data from the register specified by RA
- */
- char read( char adress);
-
- /**
- * Read multtiple regigsters from the device, more efficient than using multiple normal reads.
- *
- * @param adress - register address to write to
- * @param length - number of bytes to read
- * @param data - pointer where the data needs to be written to
- */
- void read( char adress, char *data, int length);
-
-
-
-
- private:
-
- I2C connection;
- char currentAcceleroRange;
- char currentGyroRange;
-
-
+/* Sensor datas to be used in program */
+extern float ax,ay,az;
+extern float gx,gy,gz;
+extern int16_t accelData[3],gyroData[3],tempData;
+extern float accelBias[3], gyroBias[3];
+
+/* Function Prototypes */
+class MPU6050
+{
+ protected:
+ public:
+ void getAres();
+ void getGres();
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
+ char readByte(uint8_t address, uint8_t subAddress);
+ void readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest);
+ void whoAmI();
+ void init();
+ void reset();
+ void readAccelData(int16_t* dest);
+ void readGyroData(int16_t* dest);
+ int16_t readTempData();
+ void calibrate(float* dest1, float* dest2);
+ void complementaryFilter(float* pitch, float* roll);
};
-
-
-#endif
+#endif
\ No newline at end of file