![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Added for gyro and testing
MPU6050.cpp
- Committer:
- moklumbys
- Date:
- 2015-02-12
- Revision:
- 4:268d3fcb92ba
- Parent:
- 3:187152513f8d
- Child:
- 5:5873df1e58be
File content as of revision 4:268d3fcb92ba:
/** * Includes */ #include "MPU6050.h" MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) { this->setSleepMode(false); //Initializations: currentGyroRange = 0; currentAcceleroRange=0; } //-------------------------------------------------- //-------------------General------------------------ //-------------------------------------------------- void MPU6050::write(char address, char data) { char temp[2]; temp[0]=address; temp[1]=data; connection.write(MPU6050_ADDRESS * 2,temp,2); } char MPU6050::read(char address) { char retval; connection.write(MPU6050_ADDRESS * 2, &address, 1, true); connection.read(MPU6050_ADDRESS * 2, &retval, 1); return retval; } void MPU6050::read(char address, char *data, int length) { connection.write(MPU6050_ADDRESS * 2, &address, 1, true); connection.read(MPU6050_ADDRESS * 2, data, length); } 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)); } 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); } 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); } //-------------------------------------------------- //----------------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); } 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; } 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; } 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]); } 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; } #ifdef DOUBLE_ACCELERO data[0]*=2; data[1]*=2; data[2]*=2; #endif } //function for getting angles from accelerometer void MPU6050::getAcceleroAngle( float *data ) { int temp[3]; this->getAccelero(temp); data[X_AXIS] = -1*atan (-1*temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading data[Y_AXIS] = -1*atan (temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //not sure what is this one :D } //-------------------------------------------------- //------------------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; } 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]); } void MPU6050::getGyro( float *data ) { int temp[3]; this->getGyroRaw(temp); if (currentGyroRange == MPU6050_GYRO_RANGE_250) { data[0]=(float)temp[0] / 301.0; data[1]=(float)temp[1] / 301.0; data[2]=(float)temp[2] / 301.0; } //7505.5 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; } float MPU6050::getTemp( void ) { float retval; retval=(float)this->getTempRaw(); retval=(retval+521.0)/340.0+35.0; return retval; }